# HERE CALCULATE the likelihood and save it to lik[i]
lik[i] <- dnorm(y[i], mean=C%*%X, sd=sqrt(SigmaY))



# CALCULATE HERE the negative log-likelihood and sum them
-sum(log(lik[!is.na(lik)]))



# PUT THE negative log-likelihood calculation in here
negloglik <- function(prm){
  # For keeping the likelihoods
  lik <- rep(NA,n)
  # Initialize the state
  X <- X0
  SigmaX <- matrix(c(1000,   0,
                        0,1000), nrow=2, byrow=TRUE)
  # Start on the iterations
  i <- 1
  # prm is a vector, so make it back to B
  B <- matrix(prm[c("B1","B2")], nrow=2)
  #
  while(i < n){
    # ---- PREDICT ----
    X <- A %*% X + B %*% g
    SigmaX <- A %*% SigmaX %*% t(A) + Sigma1
    SigmaY <- C %*% SigmaX %*% t(C) + Sigma2
    # ---- Step ----
    i <- i + 1
    # The likelihood
    lik[i] <- dnorm(y[i], mean=C%*%X, sd=sqrt(SigmaY))
    # ---- UPDATE ----
    K <- SigmaX %*% t(C) %*% solve(SigmaY)
    X <- X + K %*% (y[i] - C %*% X)
    SigmaX <- SigmaX - K %*% SigmaY %*% t(K)
  }
  -sum(log(lik[!is.na(lik)]))
}