R code for implementing the Gauss-Hermite Kalman filter

This blog post will demonstrate how to implement the Gauss-Hermite Kalman Filter (GHKF) in R.
Similar to the Extended Kalman Filter (EKF) and the Unscented Kalman Filter (UKF), the GHKF may be used for solving nonlinear filtering problems.

Gauss-Hermite Kalman filter

The R code below implements the Gauss-Hermite Kalman Filter as described by Simo Särkkä in his 2013 book Bayesian filtering and smoothing.

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

library(fastGHQuad)

#The R functions for the Gauss-Hermite Kalman filter (GHKF) and its smoother
#can be downloaded from: 
source("http://www.datall-analyse.nl/R/GHKF.R")
#Take a look at the functions GHKF (=the GHKF) and GHKFsmooth (=the
#RTS smoother), and notice that at the beginning of the scripts you will find
#a description of the functions' arguments.
GHKF
GHKFsmooth



##EXAMPLE 1

#The following example is similar to an example discussed 
#by Arasaratnam, Haykin, and Elliott in their 2007
#paper "Discrete-Time Nonlinear Filtering Algorithms Using
#Gauss-Hermite Quadrature".

#In this example we will estimate the state of the following
#nonlinear discrete-time system:
#x(k+1) = 0.2*x(k) + 0.01*x(k)^2 + 8*cos(1.2*(k+1)) + 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 + V
#where V is the measurement noise (variance). We will set V to 0.01.



##TRUE STATES AND MEASUREMENTS

#sample time
dT <- 1
#you may change the value of dT and see how it influences
#the behavior of the Gauss-Hermite Kalman filter

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

#state at t=0
X0 <- 0

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

#measurement noise (standard deviation)
Vk <- sqrt(.01)

#parameters in the dynamic model
phi1 <- 0.2
phi2 <- 0.01

#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[1] <- X0

for (i in 2:(length(t)+1)) {
  Xk[i] <- phi1*Xk[i-1] + phi2*Xk[i-1]^2 + 8*cos(1.2*(i)) + rnorm(1)*Wk
  Yk[i-1] <- Xk[i]^2 + 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



##GAUSS-HERMITE KALMAN FILTER (GHKF)

#Dynamic model:
#specifying 3 states, namely [x, phi1, phi2].
#Note that we augmented the parameters phi1 and phi2 onto the state vector.
#In that way, we are able to estimate these two parameters.
#Also note that the specified initial state estimates (at t=0) below for x
#and phi1 deviate from the values that were used for generating the
#data (i.e., 0 and 0.2, respectively).
#You may change these initial state estimates too and see how they
#influence the behavior of the Gauss-Hermite Kalman filter.
ex1 <- list(m0=c(.5, 0, 0.01), #initial state estimates
            #error covariance 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(5, 1, 1e-4)),
            #measurement noise
            V=Vk^2,
            #process noise
            W=diag(c(Wk^2, 0, 0)))

#Specify the state transition function:
#WARNING: always use arguments x and k when specifying the GGfunction
GGfunction <- function (x, k){
  xk <- x[1]; phi1 <- x[2]; phi2 <- x[3]
  c(phi1*xk + phi2*xk^2 + 8*cos(1.2*(k+1)),
    phi1,
    phi2)}

#Specify the observation/measurement function:
#WARNING: always use arguments x and k when specifying the FFfunction
FFfunction <- function (x, k){
  xk <- x[1]; phi1 <- x[2]; phi2 <- x[3]
  xk^2}


##Compute the filtered (a posteriori) state estimates with the GHKF
#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 phi2 from 1e-4 to 1e-5)
ghkf1 <- GHKF(y=dataEx1, mod=ex1, order=3, sqrtMethod="svd",
              GGfunction=GGfunction, FFfunction=FFfunction)

#It is also possible to compute the matrix square root with the Cholesky method.
ghkf1 <- GHKF(y=dataEx1, mod=ex1, order=3, sqrtMethod="Cholesky",
              GGfunction=GGfunction, FFfunction=FFfunction)



#As a comparison, we will also fit the Unscented Kalman Filter (UKF) to the data
source("http://www.datall-analyse.nl/R/UKF.R") #download UKF

#fit the UKF
ukf1 <- UKF(y=dataEx1, mod=ex1, sqrtMethod="Cholesky", kappa=0,
            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(ghkf1$m, ukf1$m, Xk)),
     ylab="x(k)", xlab="Time, k")
lines(c(0,t), ghkf1$m[,1], lty=2, col="blue", lwd=1)
lines(c(0,t), ukf1$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", "GHKF", "UKF"),
       bty="n", y.intersp=1.2, cex=.7)

#error plot for x
rangeError <- max(cbind(abs(Xk-ghkf1$m[,1]), abs(Xk-ukf1$m[,1])))
plot(c(0,t), abs(Xk-ghkf1$m[,1]), type="l", lty=1, col="blue",
     ylim=c(-1, rangeError), ylab="Error", xlab="Time, k")
lines(c(0,t), abs(Xk-ukf1$m[,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("GHKF", "UKF"),
       bty="n", y.intersp=1.2, cex=.7)



#compute the confidence intervals for the filtered state estimate of the GHKF

#95% ci for x
seFX <- sqrt(unlist(lapply(ghkf1$C, function(x) diag(x)[1])))
ciFX <- ghkf1$m[,1] + qnorm(.05/2)*seFX%o%c(1, -1)
#95% ci for ph1
seFP1 <- sqrt(unlist(lapply(ghkf1$C, function(x) diag(x)[2])))
ciFP1 <- ghkf1$m[,2] + qnorm(.05/2)*seFP1%o%c(1, -1)
#95% ci for ph2
seFP2 <- sqrt(unlist(lapply(ghkf1$C, function(x) diag(x)[3])))
ciFP2 <- ghkf1$m[,3] + qnorm(.05/2)*seFP2%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)

#plot confidence intervals for phi1
plot(c(0,t), rep(0.2, length(t)+1), type="l", col=c(gray(level=.7)), lwd=1.5,
     ylim=(range(ciFP1)),
     ylab="x(k)", xlab="Time, k")
lines(c(0,t), ciFP1[,1], lty=2, col="blue")
lines(c(0,t), ciFP1[,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)

#plot confidence intervals for phi2
plot(c(0,t), rep(0.01, length(t)+1), type="l", col=c(gray(level=.7)), lwd=1.5,
     ylim=(range(ciFP2)),
     ylab="x(k)", xlab="Time, k")
lines(c(0,t), ciFP2[,1], lty=2, col="blue")
lines(c(0,t), ciFP2[,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)



##Filtered versus smoothed state estimate of the GHKF

#RTS smoother for the EKF
ghkf1s <- GHKFsmooth(ghkf1)

#plot filtered and smoothed states for x
plot(c(0, t), Xk, type="l", col=c(gray(level=.5)), 
     ylab="x(k)", xlab="Time, k")
lines(c(0, t), ghkf1$m[,1], lty=2, col="blue", lwd=1)
lines(c(0, t), ghkf1s$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 GHKF", "smoothed GHKF"),
       bty="n", y.intersp=1.2, cex=.7)


##Compute the confidence intervals for the smoothed state estimate of the GHKF

#95% ci for x
seSX <- sqrt(unlist(lapply(ghkf1s$S, function(x) diag(x)[1])))
ciSX <- ghkf1s$s[,1] + qnorm(.05/2)*seSX%o%c(1, -1)
#95% ci for ph1
seSP1 <- sqrt(unlist(lapply(ghkf1s$S, function(x) diag(x)[2])))
ciSP1 <- ghkf1s$s[,2] + qnorm(.05/2)*seSP1%o%c(1, -1)
#95% ci for ph2
seSP2 <- sqrt(unlist(lapply(ghkf1s$S, function(x) diag(x)[3])))
ciSP2 <- ghkf1s$s[,3] + qnorm(.05/2)*seSP2%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(ciSX),
     ylab="x(k)", xlab="Time, k")
lines(c(0, t), ciSX[,1], lty=2, col="blue")
lines(c(0, t), ciSX[,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)

#plot confidence intervals for phi1
plot(c(0,t), rep(0.2, length(t)+1), type="l", col=c(gray(level=.7)), lwd=1.5,
     ylim=c(min(ciSP1[,1])-.2, max(ciSP1[,2])+.2),
     ylab="x(k)", xlab="Time, k")
lines(c(0, t), ciSP1[,1], lty=2, col="blue")
lines(c(0, t), ciSP1[,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)

#plot confidence intervals for phi2
plot(c(0,t), rep(0.01, length(t)+1), type="l", col=c(gray(level=.7)), lwd=1.5,
     ylim=c(min(ciSP2[,1])-.02, max(ciSP2[,2])+.02),
     ylab="x(k)", xlab="Time, k")
lines(c(0, t), ciSP2[,1], lty=2, col="blue")
lines(c(0, t), ciSP2[,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)





##EXAMPLE 2

#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^2 in
#this example.

#Note, that the nonlinearity of this dynamic system is more severe
#than that of the dynamic system in Example 1.

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



##TRUE STATES AND MEASUREMENTS

#sample time
dT <- 1
#you may change the value of dT and see how it influences
#the behavior of the Gauss-Hermite Kalman filter

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

#state at t=0
X0 <- 0

#process noise (standard deviation)
Wk <- 10

#measurement noise (standard deviation)
Vk <- 1

#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[1] <- 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
dataEx2 <- Yk



##GAUSS-HERMITE KALMAN FILTER (GHKF)

#Dynamic model:
#specifying 1 state, namely [x].
#Note that the specified initial state estimates (at t=0) below for x
#deviates from the value that was used for generating the data (i.e., 0).
#You may change this initial state estimate too and see how it
#influences the behavior of the Gauss-Hermite Kalman filter.
ex2 <- list(m0=2, #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 Kalman filter
            C0=5^2,
            #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 GHKF
#If the filter generates an error, then try to decrease the 
#error covariance of the initial state estimate (as specified in C0).
#(For instance, set the value for x from 5^2 to 10)
ghkf2 <- GHKF(y=dataEx2, mod=ex2, order=10, sqrtMethod="svd",
              GGfunction=GGfunction, FFfunction=FFfunction)

#It is also possible to compute the matrix square root with the Cholesky method.
ghkf2 <- GHKF(y=dataEx2, mod=ex2, order=10, sqrtMethod="Cholesky",
              GGfunction=GGfunction, FFfunction=FFfunction)


#As a comparison, we will also fit the Unscented Kalman Filter (UKF) to the data
ukf2 <- UKF(y=dataEx2, mod=ex2, sqrtMethod="Cholesky", kappa=2,
            GGfunction=GGfunction, FFfunction=FFfunction)



#plot the filtered state estimate
plot(c(0,t), Xk, type="o", col=c(gray(level=.5)),
     ylim=range(cbind(ghkf2$m, ukf2$m, Xk)),
     ylab="x(k)", xlab="Time, k")
lines(c(0,t), ghkf2$m[,1], lty=2, col="blue", lwd=1)
lines(c(0,t), ukf2$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", "GHKF", "UKF"),
       bty="n", y.intersp=1.2, cex=.7)

#error plot
rangeError <- max(cbind(abs(Xk-ghkf2$m), abs(Xk-ukf2$m)))
plot(c(0,t), abs(Xk-ghkf2$m), type="l", lty=1, col="blue",
     ylim=c(-1, rangeError), ylab="Error", xlab="Time, k")
lines(c(0,t), abs(Xk-ukf2$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("GHKF", "UKF"),
       bty="n", y.intersp=1.2, cex=.7)



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

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

#plot confidence intervals
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)



##Filtered versus smoothed state estimates of the GHKF

#RTS smoother for the GHKF
ghkf2s <- GHKFsmooth(ghkf2)

#plot filtered and smoothed states
plot(c(0, t), Xk, type="l", col=c(gray(level=.5)), 
     ylab="x(k)", xlab="Time, k")
lines(c(0, t), ghkf2$m, lty=2, col="blue", lwd=1)
lines(c(0, t), ghkf2s$s, 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 GHKF", "smoothed GHKF"),
       bty="n", y.intersp=1.2, cex=.7)


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

#95% ci for x
seSX <- sqrt(unlist(lapply(ghkf2s$S, function(x) diag(x)[1])))
ciSX <- ghkf2s$s[,1] + qnorm(.05/2)*seSX%o%c(1, -1)

#plot confidence intervals
plot(c(0, t), Xk, type="l", col=c(gray(level=.7)), lwd=1.5,
     ylim=range(ciSX),
     ylab="x(k)", xlab="Time, k")
lines(c(0, t), ciSX[,1], lty=2, col="blue")
lines(c(0, t), ciSX[,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)

In my next two posts I will show 1) how to estimate unknown parameters of a GHKF-model using likelihood maximization, and 2) how to predict future states and observations with the GHKF.