Rでリアルタイムでカルマンフィルタを使う

まだうまくいってない
lengthとかnrowとかがこんがらがってきた

途中のコード

window=20;
update = 0.5;
df <- data.frame();
#kalman parameter
dt <- matrix(0)
ct <- matrix(0)
Zt <- matrix(1)
Tt <- matrix(1)
a0 <- 0
P0 <- matrix(0.01);
alpha <- cumsum(2.0 * rnorm(20))
y     <- alpha + rnorm(20)
#fit.fkf <- NULL
#fkf.obj <- NULL
#estimate <- NULL 
# objective function
objective <- function(par, ...){
          -fkf(HHt = matrix(par[1]), GGt = matrix(par[2]), ...)$logLik
}
updateObs<- function(y,alpha){
  alpha[length(alpha)+1] <- alpha + 2.0*rnorm(20)
  y[length(y)+1] <- rnorm(1)
  return(list(alpha=alpha,y=y))
}
updateKF <- function(y,alpha,ests){
  updated <- updateObs(y,alpha)
  alpha <- updated$alpha
  y <- updated$y
  fit.fkf <- optim(c(HHt = 4, GGt = 1), fn = objective,
                   yt = rbind(y), a0 = a0, P0 = P0, dt = dt, ct = ct, Zt = Zt, Tt = Tt)
  fkf.obj <- fkf(a0, P0, dt, ct, Tt, Zt, HHt = matrix(fit.fkf$par[1]),
                 GGt = matrix(fit.fkf$par[2]), yt = rbind(y))
  ests[length(ests)] <- fkf.obj$att[1,]
  return(list(alpha=alpha,y=y,ests=ests))
}
## Plot data together with filtered value:
i=0
while(TRUE) {
    flush.console()
    updated <- updateKF(y,alpha,ests)
    alpha <- updated$alpha
    y <- updated$y
    ests <- updated$ests
    df <- rbind(df,data.frame(time=nrow(df)+1,y="y(length(y))",est="ests(length(ests))",real="alpha(length(alpha))"))
    if(nrow(df) > window){
      plot(df$time,df$y,type='l',xlim=c(i-window,i))
      lines(ests,col = "blue")
      legend("bottomright", c("original data", "estimation"), col = c("black", "blue"), lty = 1)
    } else {
      plot(df$time,df$y,type='l',xlim=c(0,window))
      message(ests)
      lines(ests,col = "blue")
      legend("bottomright", c("original data", "estimation"), col = c("black", "blue"), lty = 1)
    }
    i <- i+1;
    Sys.sleep(update)
}