## ----------------------------------------------------------------
# Task1: Write the missing parts of the Kalman filter
## ----------------------------------------------------------------
i <- 1
while(i < n){
  # ---- PREDICT ----
  X <- A %*% X + B %*% g
  SigmaX <- A %*% SigmaX %*% t(A) + Sigma1
  SigmaY <- C %*% SigmaX %*% t(C) + Sigma2
  # Keep the prediction
  ypred[i+1] <- C %*% X
  # ---- Step ----
  i <- i + 1
  # ---- UODATE ----
  K <- SigmaX %*% t(C) %*% solve(SigmaY)
  X <- X + K %*% (y[i] - C %*% X)
  SigmaX <- SigmaX - K %*% SigmaY %*% t(K)
  # Keep the reconstruction
  yhat[i] <- C %*% X
}

## ----------------------------------------------------------------
# Play around with it
## ----------------------------------------------------------------

## ----------------------------------------------------------------
# Add noise
## ----------------------------------------------------------------
# Then also set the variances again
(Sigma1 <- diag(c( 0.05^2, (0.05/fps)^2)))
(Sigma2 <- matrix(1.1^2))


## ----------------------------------------------------------------
# Add a step to y
## ----------------------------------------------------------------
# Then also set the variances again
(Sigma1 <- diag(c( 1.25^2, (1.25/fps)^2)))
(Sigma2 <- matrix(2.1^2))



## ----------------------------------------------------------------
# Estimate the g
## ----------------------------------------------------------------
# Set the constant matrices and vectors of the Kalman filter
# Transition matrix
(A <- matrix(c(1,1,-0.5,
               0,1,-1,
               0,0,1), nrow=3, byrow=TRUE))