1## Improved version supplied by one referee, not shown in paper 2KalmanRimp <- function(pos) { 3 4 kalmanfilter <- function(z) { 5 ## predicted state and covriance 6 xprd <- xest %*% A 7 pprd <- crossprod(pest %*% A, A) + Q 8 9 ## estimation 10 B <- crossprod(H, pprd) 11 S <- B %*% H + R 12 13 ## kalmangain <- (S \ B)' 14 kalmangain <- solve(S, B) 15 16 ## estimated state and covariance, assign to vars in parent env 17 xest <<- xprd + (z - xprd %*% H) %*% kalmangain 18 pest <<- pprd - pprd %*% H %*% kalmangain 19 20 ## compute the estimated measurements 21 y <- xest %*% H 22 } 23 24 dt <- 1 25 A <- matrix( c( 1, 0, dt, 0, 0, 0, # x 26 0, 1, 0, dt, 0, 0, # y 27 0, 0, 1, 0, dt, 0, # Vx 28 0, 0, 0, 1, 0, dt, # Vy 29 0, 0, 0, 0, 1, 0, # Ax 30 0, 0, 0, 0, 0, 1), # Ay 31 6, 6, byrow=FALSE) 32 H <- matrix( c(1, 0, 0, 0, 0, 0, 33 0, 1, 0, 0, 0, 0), 34 6, 2, byrow=FALSE) 35 Q <- diag(6) 36 R <- 1000 * diag(2) 37 38 N <- nrow(pos) 39 y <- matrix(NA, N, 2) 40 41 xest <- matrix(0, 1, 6) 42 pest <- matrix(0, 6, 6) 43 44 for (i in 1:N) { 45 y[i,] <- kalmanfilter(pos[i,,drop=FALSE]) 46 } 47 48 invisible(y) 49} 50