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