R code for estimating the position of a moving object using the Kalman filter

The following R code may be used for estimating the position (and velocity) of a moving object by means of the Kalman filter.

The R code uses the Kalman filter implemented in the dlm package.Kalman filter

Two very readable introductions to the Kalman filter are:

  • Faragher, R. (2012) Understanding the basis of the Kalman filter via a simple and intuitive derivation, IEEE Signal Processing Magazine, 29, pp. 128-132
  • Simon, D. (2001) Kalman filtering, Embedded Systems Programming, 14, pp. 72-79 (available from Dan Simon’s website)

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(dlm)
library(mvtnorm)

##EXAMPLE 1: two dimensional projectile motion

#if we assume that the projectile's motion is not influenced (controlled)
#by factors such as drag and wind gusts, then the equations of
#motions are (in Cartesian coordinates):

#for the x-direction:
#position = x0 + v0*cos(alpha)*t
#velocity = v0*cos(alpha)
#acceleration = 0

#for the y-direction:
#position = y0 + v0*sin(alpha)*t - g*t
#velocity = v0*sin(alpha) - .5*g*t^2
#acceleration = -g

#where x0 is the x-position at t=0, y0 the y-position at t=0,
#v0 the initial (launching) velocity, alpha the angle with the x-axis,
#and g the acceleration in the y direction (which is 9.81)

#see also Wikipedia's article on projectile motion
#at: https://en.wikipedia.org/wiki/Projectile_motion

#in Example 1, we assume that we measure at each time step
#- the position in the x and y direction
#- the velocity in the x and y direction



##TRUE STATE
#x-position at t=0
x0 <- 0

#y-position at t=0
y0 <- 0

#velocity at t=0
v0 <- 30

#acceleration in y-direction
g <- 9.81

#launching angle (in degrees)
alphaDeg <- 30
#convert degrees to radians (by multiplying with pi/180)
alpha <- alphaDeg*(pi/180)

#flight time
2*v0/g*sin(alpha)

#observation times
t <- seq(0.1, 3, .1)

#compute x-position at the observation times
x <- x0 + v0*cos(alpha)*t

#compute y-position at the observation times
y <- y0 + v0*sin(alpha)*t - .5*g*t^2

#trajectory of the projectile
plot(x, y, type="l", xlab="x-position", ylab="y-position")

#compute velocity in the x-direction at the observation times
vx <- v0*cos(alpha)

#velocity in x-direction
plot(t, rep(vx, length(t)), type="l", xlab="time", ylab="vx")

#compute velocity in the y-direction at the observation times
vy <- v0*sin(alpha) - g*t

#velocity in y-direction
plot(t, vy, type="l", xlab="time", ylab="vy")



##MEASUREMENTS

#assume that the measurement noise is normally distributed
#with mean=0 and sd=.5

#x-position
xm <- x0 + v0*cos(alpha)*t + rnorm(length(t), 0, .5)

#y-position
ym <- y0 + v0*sin(alpha)*t - .5*g*t^2 + rnorm(length(t), 0, .5)

#velocity in x-direction
vxm <- rep(v0*cos(alpha), length(t)) + rnorm(length(t), 0, .5)

#velocity in y-direction
vym <- v0*sin(alpha) - g*t + rnorm(length(t), 0, .5)

#measured trajectory of the projectile
plot(x, y, type="l", xlab="x-position", ylab="y-position", ylim=c(-2, 14))
lines(xm, ym, type="o", lty=2, col="red")
legend("topleft", pch=c(NA, 1), lty=c(1, 2),
       col=c("black", "red"), legend=c("true state", "measured"),
       bty="n", y.intersp=1.2)

#store measurement data
dataEx1 <- cbind(xm, vxm, ym, vym)



##KALMAN FILTER
dt <- .1 #time step

#dynamic linear model
#specifying 5 states, namely [x, vx, y, vy, uy]
#where x is the x-position, vx the velocity in the x direction,
#y the y-position, vy the velocity in the y direction, and uy
#the acceleration in the y direction
ex1 <- dlm(m0=c(0, v0*cos(alpha), 0, v0*sin(alpha), -g), #initial state estimates
           #error covariances of the initial state estimates:
           #this vector reflects the uncertainty in our initial state estimates
           #you may change the values in this vector and see how they influence
           #the behavior of the Kalman filter
           C0=diag(c(rep(.5,4), 0)),
           #observation matrix
           FF=matrix(c(1,0,0,0,0,
                       0,1,0,0,0,
                       0,0,1,0,0,
                       0,0,0,1,0), nrow=4, byrow=TRUE),
           #measurement noise
           V=diag(c(rep(.5^2,4))),
           #state transition matrix
           GG=matrix(c(1,dt,0,0,0,
                       0,1,0,0,0,
                       0,0,1,dt,.5*dt^2,
                       0,0,0,1,dt,
                       0,0,0,0,1), nrow=5, byrow=TRUE),
           #process noise
           W=diag(rep(0,5)))

#note that all covariances in the process noise matrix (W) are set to zero
#this makes sense since, for instance, the change in x-position at each time
#step is fully explained by our physical model describing the projectile's motion
#furthermore, we assumed that there were no factors (such as random wind gusts)
#that could possibly influence the x-position at each time step in some
#unpredictable (random) way
#see Example 3 for a model were not all entries in the W matrix are set to zero

##compute the filtered (a posteriori) state estimates
filteredState <- dlmFilter(dataEx1, ex1)

#plot the filtered state estimates 
plot(x, y, type="l", lwd=1.5, xlab="x-position", ylab="y-position",
     xlim=c(-5, 85), ylim=c(-2, 14), col=gray(level=.7))
lines(xm, ym, type="o", lty=2, col="red", cex=.7)
points(filteredState$m[,1], filteredState$m[,3], col="blue", cex=.7)
legend("topleft", pch=c(NA, 1, 1), lty=c(1, 2, NA), lwd=c(1, 1, NA),
       col=c(gray(level=.7), "red", "blue"),
       legend=c("true state", "measured", "filtered state"),
       bty="n", y.intersp=1.2, cex=.7)

#compute the confidence intervals for the filtered state estimates
varcovFilteredState <- dlmSvd2var(filteredState$U.C, filteredState$D.C)
#95% ci for x-position
seFX <- sqrt(unlist(lapply(varcovFilteredState, function(x) x[1,1])))
ciFX <- filteredState$m[,1] + qnorm(.05/2)*seFX%o%c(1, -1)
#95% ci for y-position
seFY <- sqrt(unlist(lapply(varcovFilteredState, function(x) x[3,3])))
ciFY <- filteredState$m[,3] + qnorm(.05/2)*seFY%o%c(1, -1)

#x-position
plot(t, x, type="l", xlab="time", ylab="x-position",
     xlim=c(0,3), ylim=c(-5, 85), col=gray(level=.7), lwd=1.5)
arrows(c(0,t), ciFX[,1], c(0,t), ciFX[,2],
       code=3, length=0.05, angle=90, col="blue")

#y-position
plot(t, y, type="l", xlab="time", ylab="y-position",
     xlim=c(0,3), ylim=c(-5, 12), col=gray(level=.7), lwd=1.5)
arrows(c(0,t), ciFY[,1], c(0,t), ciFY[,2],
       code=3, length=0.05, angle=90, col="blue")



##diagnostics

#for the x-position

#raw residuals (innovations)
plot(t, residuals(filteredState, sd=FALSE, type="raw")[,1], type="o",
     ylab="raw residuals")
abline(h=0, lty=2, col="darkgray")

#standardized residuals
sresx <- residuals(filteredState, sd=FALSE, type="standardized")[,1]
plot(t, sresx, type="o", ylab="standardized residuals")
abline(h=0, lty=2, col="darkgray")

#qq-plot for standardized residuals
qqnorm(sresx)
abline(a=0, b=1, col="darkgray", lty=2)

#acf plot for standardized residuals
acf(residuals(filteredState, sd=FALSE)[,1], main="x-position")



#combined plots for all states together

#standardized residuals
par(mfrow=c(2, 2))
plot(t, residuals(filteredState, sd=FALSE)[,1], type="o",
     ylab="raw residuals", main="x-position")
plot(t, residuals(filteredState, sd=FALSE)[,2], type="o",
     ylab="raw residuals", main="velocity in x-direction")
plot(t, residuals(filteredState, sd=FALSE)[,3], type="o",
     ylab="raw residuals", main="yx-position")
plot(t, residuals(filteredState, sd=FALSE)[,4], type="o",
     ylab="raw residuals", main="velocity in y-direction")
par(mfrow=c(1, 1))

#acf-plots
par(mfrow=c(2, 2))
acf(residuals(filteredState, sd=FALSE)[,1], main="x-position")
acf(residuals(filteredState, sd=FALSE)[,2], main="velocity in x-direction")
acf(residuals(filteredState, sd=FALSE)[,3], main="y-position")
acf(residuals(filteredState, sd=FALSE)[,4], main="velocity in y-direction")
par(mfrow=c(1, 1))



##compute smoothed state estimates

#apply fixed-interval smoothing
smoothState <- dlmSmooth(filteredState)
plot(x, y, type="l", xlab="x-position", ylab="y-position",
     xlim=c(-5, 85), ylim=c(-2, 14))
lines(filteredState$m[,1], filteredState$m[,3], type="o", lty=2, col="blue")
lines(smoothState$s[,1], smoothState$s[,3], type="o", lty=2, col="darkgreen")
legend("topleft", pch=c(NA, 1, 1), lty=c(1, 2, 2), lwd=c(1, 1, 1),
       col=c("black", "blue", "darkgreen"),
       legend=c("true state", "filtered state", "smoothed state"),
       bty="n", y.intersp=1.2, cex=.7)

#confidence intervals for smoothed state estimates
varcovSmoothState <- dlmSvd2var(smoothState$U.S, smoothState$D.S)
#95% ci for x-position
seSX <- sqrt(unlist(lapply(varcovSmoothState, function(x) x[1,1])))
ciSX <- smoothState$s[,1] + qnorm(.05/2)*seSX%o%c(1, -1)
#95% ci for y-position
seSY <- sqrt(unlist(lapply(varcovSmoothState, function(x) x[3,3])))
ciSY <- smoothState$s[,3] + qnorm(.05/2)*seSY%o%c(1, -1)

#plot x-position
plot(t, x, type="l", xlab="time", ylab="x-position",
     xlim=c(0,3), ylim=c(-5, 85), col=gray(level=.7), lwd=1.5)
arrows(c(0,t), ciSX[,1], c(0,t), ciSX[,2],
       code=3, length=0.05, angle=90, col="blue")

#plot y-position
plot(t, y, type="l", xlab="time", ylab="y-position",
     xlim=c(0,3), ylim=c(-5, 12), col=gray(level=.7), lwd=1.5)
arrows(c(0,t), ciSY[,1], c(0,t), ciSY[,2],
       code=3, length=0.05, angle=90, col="blue")


##compute forecasts
dataEx1pred <- cbind(xm,vxm,ym,vym)[1:20,]
filteredStateForecast <- dlmFilter(dataEx1pred,ex1)
forecastState <- dlmForecast(filteredStateForecast, nAhead=10)

#plot forecasts
plot(x, y, type="l", lty=1, xlab="x-position", ylab="y-position",
     xlim=c(-5, 85), ylim=c(-2, 12), col=gray(level=.7), lwd=1.5)
points(filteredStateForecast$m[,1], filteredStateForecast$m[,3], col="blue",
       cex=.7)
points(forecastState$f[,1], forecastState$f[,3], col="red", cex=.7)
legend("topleft", pch=c(NA, 1, 1), lty=c(1, NA, NA),
       col=c(gray(level=.7), "blue", "red"),
       legend=c("true state", "filtered state", "forecast"),
       bty="n", y.intersp=1.2, cex=.7)

#prediction intervals for forecasts
#95% ci for x-position
seFoX <- sqrt(unlist(lapply(forecastState$Q, function(x) x[1,1])))
ciFoX <- forecastState$f[,1] + qnorm(.05/2)*seFoX%o%c(1, -1)
#95% ci for y-position
seFoY <- sqrt(unlist(lapply(forecastState$Q, function(x) x[3,3])))
ciFoY <- forecastState$f[,3] + qnorm(.05/2)*seFoY%o%c(1, -1)

#plot forecasts x-position
plot(t[21:30], x[21:30], type="l", xlab="time", ylab="x-position",
     ylim=c(40, 85), col=gray(level=.7), lwd=1.5)
arrows(t[21:30], ciFoX[,1], t[21:30], ciFoX[,2],
       code=3, length=0.05, angle=90, col="red")

#plot forecasts y-position
plot(t[21:30], y[21:30], type="l", xlab="time", ylab="y-position",
     ylim=c(-1, 12), col=gray(level=.7), lwd=1.5)
arrows(t[21:30], ciFoY[,1], t[21:30], ciFoY[,2],
       code=3, length=0.05, angle=90, col="red")



##EXAMPLE 2
#similar to Example 1, but in this example we assume that we only
#measured at each time step the x and y position of the projectile 

dataEx2 <- cbind(xm, ym) #measurements of the x and y position



##KALMAN FILTER
dt <- .1 #time step

#specify dynamic linear model
ex2 <- dlm(m0=c(0, v0*cos(alpha), 0, v0*sin(alpha), g),
           C0=diag(c(rep(.5,4), 0)),
           FF=matrix(c(1,0,0,0,0,
                       0,0,1,0,0), nrow=2, byrow=TRUE),
           V=diag(c(rep(.5^2,2))),
           GG=matrix(c(1,dt,0,0,0,
                       0,1,0,0,0,
                       0,0,1,dt,-.5*dt^2,
                       0,0,0,1,-dt,
                       0,0,0,0,1), nrow=5, byrow=TRUE),
           W=diag(rep(0,5)))



##compute filtered (a posteriori) state estimates
filteredState2 <- dlmFilter(dataEx2, ex2)

#plot the filtered state estimates 
plot(x, y, type="l", lwd=1.5, xlab="x-position", ylab="y-position",
     xlim=c(-5, 85), ylim=c(-2, 14), col=gray(level=.7))
lines(xm, ym, type="o", lty=2, col="red", cex=.7)
points(filteredState2$m[,1], filteredState2$m[,3], col="blue", cex=.7)
legend("topleft", pch=c(NA, 1, 1), lty=c(1, 2, NA), lwd=c(1.5, 1, NA),
       col=c(gray(level=.7), "red", "blue"),
       legend=c("true state", "measured", "filtered state"),
       bty="n", y.intersp=1.2, cex=.7)

#confidence intervals for the filtered state estimates
varcovFilteredState2 <- dlmSvd2var(filteredState2$U.C, filteredState2$D.C)
#95% ci for x-position
seFX2 <- sqrt(unlist(lapply(varcovFilteredState2, function(x) x[1,1])))
ciFX2 <- filteredState2$m[,1] + qnorm(.05/2)*seFX2%o%c(1, -1)
#95% ci for y-position
seFY2 <- sqrt(unlist(lapply(varcovFilteredState2, function(x) x[3,3])))
ciFY2 <- filteredState2$m[,3] + qnorm(.05/2)*seFY2%o%c(1, -1)

#for the x-position
plot(t, x, type="l", xlab="time", ylab="x-position",
     xlim=c(0,3), ylim=c(-5, 85), col=gray(level=.7), lwd=1.5)
arrows(c(0,t), ciFX2[,1], c(0,t), ciFX2[,2],
       code=3, length=0.05, angle=90, col="blue")

#for the y-position
plot(t, y, type="l", xlab="time", ylab="y-position",
     xlim=c(0,3), ylim=c(-5, 12), col=gray(level=.7), lwd=1.5)
arrows(c(0,t), ciFY2[,1], c(0,t), ciFY2[,2],
       code=3, length=0.05, angle=90, col="blue")


##compute forecasts
dataEx2pred <- cbind(xm, ym)[1:20,]
filteredStateForecast2 <- dlmFilter(dataEx2pred, ex2)
forecastState2 <- dlmForecast(filteredStateForecast2, nAhead=10)

#plot forecasts
plot(x, y, type="l", lty=1, xlab="x-position", ylab="y-position",
     xlim=c(-5, 85), ylim=c(-2, 12), col=gray(level=.7), lwd=1.5)
points(filteredStateForecast2$m[,1], filteredStateForecast2$m[,3], col="blue",
       cex=.7)
points(forecastState2$f[,1], forecastState2$f[,2], col="red", cex=.7)
legend("topleft", pch=c(NA, 1, 1), lty=c(1, NA, NA),
       col=c(gray(level=.7), "blue", "red"),
       legend=c("true state", "filtered state", "forecast"),
       bty="n", y.intersp=1.2, cex=.7)

#prediction intervals for forecasts
#95% ci for x-position
seFoX2 <- sqrt(unlist(lapply(forecastState2$Q, function(x) x[1,1])))
ciFoX2 <- forecastState2$f[,1] + qnorm(.05/2)*seFoX2%o%c(1, -1)
#95% ci for y-position
seFoY2 <- sqrt(unlist(lapply(forecastState2$Q, function(x) x[2,2])))
ciFoY2 <- forecastState2$f[,2] + qnorm(.05/2)*seFoY2%o%c(1, -1)

#plot forecasts x-position
plot(t[21:30], x[21:30], type="l", xlab="time", ylab="x-position",
     ylim=c(40, 85), col=gray(level=.7), lwd=1.5)
arrows(t[21:30], ciFoX2[,1], t[21:30], ciFoX2[,2],
       code=3, length=0.05, angle=90, col="red")

#plot forecasts y-position
plot(t[21:30], y[21:30], type="l", xlab="time", ylab="y-position",
     ylim=c(-1, 12), col=gray(level=.7), lwd=1.5)
arrows(t[21:30], ciFoY2[,1], t[21:30], ciFoY2[,2],
       code=3, length=0.05, angle=90, col="red")



#EXAMPLE 3: one dimensional object motion with process noise

#in this example we assume that an object (a vehicle) is constrained
#to move in one dimension
#furthermore, we assume that the object's motion is controlled (influenced)
#by unpredictable (random) factors such as wind gusts and potholes
#these factors control the object's motion at each time step in an
#unpredictable way (they add random noise to the object's motion)
#note that these factors influence the object's motion at each time
#step by accelerating it

#the equations of motion for our object are:
#position = p0 + p0*t + .5*u*t^2 + pnoise
#velocity = v0 + u*t + vnoise
#acceleration = u

#where p0 is the position at t=0, pnoise the position noise, 
#v0 the velocity at t=0, vnoise the velocity noise,
#and u the mean acceleration during the object's motion

#in this example, we assume that we only measure the object's position 
#at each time step
#note that Dan Simon, in his paper "Kalman filtering", tackles the same problem
#as presented in this example 



##TRUE STATE
#initial position (at t=0)
p0 <- 0

#initial velocity (at t=0)
v0 <- 0

#mean acceleration
u <- 1

#standard deviation acceleration
usd <- .2

#standard deviation of position measurement
msd <- 10

#time step
dt <- .1

#observation times
t <- seq(0.1, 20, dt)

#state transition matrix
A <- matrix(c(1,dt,
              0,1), nrow=2, byrow=TRUE)

#control input matrix
B <- c(.5*dt^2, dt)

#observation matrix
H <- c(1, 0)

#variance-covariance matrix for the process noise
#(this process noise is due to the random character of the acceleration)
vcp <- B%*%t(B)*usd^2 #using the delta method

#simulate data
x <- matrix(NA, nrow=length(t)+1, ncol=2)
y <- matrix(NA, nrow=length(t)+1, ncol=1)
x[1,] <- c(p0,v0)

for (i in 2:(length(t)+1)) {
  x[i,] <- A%*%x[(i-1),] + B*u + t(rmvnorm(n=1, sigma=vcp))
  y[i,] <- H%*%x[i,] + msd*rnorm(1)}

#trajectory
plot(c(0,t), y[,1], type="l", col=gray(level=.7), xlab="time", ylab="position")
lines(c(0,t), x[,1], col="red", lty=2)
legend("topleft", lty=c(1, 2),
       col=c(gray(level=.7), "red"), legend=c("true state", "measured"),
       bty="n", y.intersp=1.2)

#velocity
plot(c(0,t), x[,2], type="l", xlab="time", ylab="velocity")



##KALMAN FILTER

#specify dynamic linear model
ex3 <- dlm(m0=c(0, 0, 1),
           C0=diag(c(rep(.5,2), 0)),
           FF=matrix(c(1,0,0), nrow=1),
           V=msd^2,
           GG=matrix(c(1,dt,.5*dt^2,
                       0,1,dt,
                       0,0,1), nrow=3, byrow=TRUE),
           W=cbind(rbind(vcp,0),0))


##compute filtered (a posteriori) state estimates
filteredState <- dlmFilter(y[-1], ex3)

#plot filtered state estimates

#position
plot(c(0,t), y, col=gray(level=.7), type="l", xlab="time", ylab="position")
lines(c(0,t), x[,1], col="red", lty=2)
lines(c(0,t), filteredState$m[,1], col="blue", lty=2)
legend("topleft", lty=c(1, 2, 2),
       col=c(gray(level=.7), "red", "blue"),
       legend=c("measured", "true state", "filtered state"),
       bty="n", y.intersp=1.2, cex=.7)

#velocity
plot(c(0,t), x[,2], col="red", type="l", xlab="time", ylab="velocity")
lines(c(0,t), filteredState$m[,2], col="blue", lty=2)
legend("topleft", lty=c(1, 2),
       col=c(gray(level=.7), "red", "blue"),
       legend=c("true state", "filtered state"),
       bty="n", y.intersp=1.2, cex=.7)

#confidence intervals for the filtered state estimates
varcovFilteredState <- dlmSvd2var(filteredState$U.C, filteredState$D.C)
#95% ci for position
seFP <- sqrt(unlist(lapply(varcovFilteredState, function(x) x[1,1])))
ciFP <- filteredState$m[,1] + qnorm(.05/2)*seFP%o%c(1, -1)
#95% ci for velocity
seFV <- sqrt(unlist(lapply(varcovFilteredState, function(x) x[2,2])))
ciFV <- filteredState$m[,2] + qnorm(.05/2)*seFV%o%c(1, -1)

#plot for position
plot(c(0,t), x[,1], type="l", xlab="time", ylab="position",
     ylim=c(-5, 200), col=gray(level=.7), lwd=1.5)
lines(c(0,t), ciFP[,1], lty=2, col="blue")
lines(c(0,t), ciFP[,2], lty=2, col="blue")

#plot for velocity
plot(c(0,t), x[,2], type="l", xlab="time", ylab="velocity",
     ylim=c(-5, 25), col=gray(level=.7), lwd=1.5)
lines(c(0,t), ciFV[,1], lty=2, col="blue")
lines(c(0,t), ciFV[,2], lty=2, col="blue")



##error plots
#compare with Dan Simon's Figures 2 and 4 

#position error plot:
#difference between the true and the measured position
#and the difference between the true position and the filtered position estimate
plot(c(0,t), y-x[,1], col=gray(level=.7), ylim=c(-35, 35), 
     type="l", xlab="time", ylab="position error")
lines(c(0,t), filteredState$m[,1]-x[,1], col=gray(level=.2))
abline(h=0, col="blue", lty=2)

#velocity error plot
plot(c(0,t), filteredState$m[,2]-x[,2], col=gray(level=.2), ylim=c(-2,2), 
     type="l", xlab="time", ylab="velocity error")
abline(h=0, col="blue", lty=2)



##diagnostics

#raw residuals (=innovations)
plot(t, residuals(filteredState, sd=FALSE, type="raw"), type="l",
     ylab="raw residuals")
abline(h=0, lty=2, col="darkgray")

#standardized residuals
sresx <- residuals(filteredState, sd=FALSE, type="standardized")
plot(t, sresx, type="l", ylab="standardized residuals")
abline(h=0, lty=2, col="darkgray")

#qq-plot for standardized residuals
qqnorm(sresx)
abline(a=0, b=1, col="darkgray", lty=2)

#acf for standardized residuals
acf(residuals(filteredState, sd=FALSE), main="position")



##compute smoothed state estimates

#fixed interval smoothing
smoothState <- dlmSmooth(filteredState)
plot(c(0,t), x[,1], col=gray(level=.7), type="l", xlab="time", ylab="position")
lines(c(0,t), filteredState$m[,1], lty=2, col="blue")
lines(c(0,t), smoothState$s[,1], lty=2, col="darkgreen")
legend("topleft", lty=rep(2,3), 
       col=c("black", "blue", "darkgreen"),
       legend=c("true state", "filtered state", "smoothed state"),
       bty="n", y.intersp=1.2)

#confidence intervals for smoothed state estimates
varcovSmoothState <- dlmSvd2var(smoothState$U.S, smoothState$D.S)
#95% ci for position
seSP <- sqrt(unlist(lapply(varcovSmoothState, function(x) x[1,1])))
ciSP <- smoothState$s[,1] + qnorm(.05/2)*seSP%o%c(1, -1)
#95% ci for velocity
seSV <- sqrt(unlist(lapply(varcovSmoothState, function(x) x[2,2])))
ciSV <- smoothState$s[,2] + qnorm(.05/2)*seSV%o%c(1, -1)

#plot position
plot(c(0,t), x[,1], type="l", xlab="time", ylab="position",
     ylim=c(-5, 200), col=gray(level=.7), lwd=1.5)
lines(c(0,t), ciSP[,1], lty=2, col="blue")
lines(c(0,t), ciSP[,2], lty=2, col="blue")

#plot velocity
plot(c(0,t), x[,2], type="l", xlab="time", ylab="velocity",
     ylim=c(-5, 25), col=gray(level=.7), lwd=1.5)
lines(c(0,t), ciSV[,1], lty=2, col="blue")
lines(c(0,t), ciSV[,2], lty=2, col="blue")