# 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. The R code below implements the Gauss-Hermite Kalman Filter as described by Simo Särkkä in his 2013 book Bayesian filtering and smoothing.

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

#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 <- 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; phi1 <- x; phi2 <- x
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; phi1 <- x; phi2 <- x
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

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