## ----------------------------------------------------------------
# Kalman filter exercise
## ----------------------------------------------------------------

# Read the position taken from a video recording
coordinates <- read.table("exercise_kalmanfilter_data.csv", header=TRUE, sep=";", dec=",")

# Use on the height
y <- coordinates$heightM
plot(y)
n <- length(y)

# Samplerate is 30 fps (in the video), so we just use units "per frame" instead of "per second"
fps <- 30
g <- 9.8 # [m/s^2]
# Make the g in meter per frame^2, which just is the acceleration, which will be added to the speed
(g <- 9.8 / (fps^2))  # [m/f^2]


## ----------------------------------------------------------------
# Set the constant matrices and vectors of the Kalman filter
# Transition matrix
(A <- matrix(c(1,1,
               0,1), nrow=2, byrow=TRUE))
# Input matrix
(B <- matrix(c(-0.5,-1), nrow=2))
# Output matrix
(C <- matrix(c(1,0), nrow=1))

# State covariance
(Sigma1 <- diag(c( 0.05^2, (0.05/fps)^2)))
# Output variance
(Sigma2 <- matrix(0.1^2))

# Initial values
# The initial state values (note the speed is set to 11 m/s, but the value is per frame)
(X0 <- c(2.1, 11/fps)) # [m , m/f]
# Initial state covariance
(SigmaX0 <- diag(c( 0.5^2, (1/fps)^2)))


## ----------------------------------------------------------------
# Initialize

# For keeping the predicted and the reconstructed output
ypred <- numeric(length(y))
yhat <- numeric(length(y))

# Set the values for t=0 (well, so i=1)
X <- X0
SigmaX <- SigmaX0
yhat[1] <- C %*% X
ypred[1] <- yhat[1]

## ----------------------------------------------------------------
# Task: Write the missing parts of the Kalman filter
## ----------------------------------------------------------------
i <- 1
while(i < n){
  # ---- PREDICT ----
  WRITE THE PREDICTION HERE
  # Keep the prediction
  ypred[i+1] <- C %*% X
  # ---- Step ----
  i <- i + 1
  # ---- UODATE ----
  WRITE THE UPDATE HERE
  # Keep the reconstruction
  yhat[i] <- C %*% X
}
# Plot it
plot(y)
lines(ypred, col=2)
lines(yhat, col=3)
legend("topright", c("Observation","Predicted","Reconstructed"), lty=1, col=1:3)


## ----------------------------------------------------------------
# Check your result with an existing implementation
## ----------------------------------------------------------------

# Use the Kalman filter from the FKF package
library("FKF")
## Running the Kalman filter with the parameters and initial values used for the simulation
kf1 <- fkf(a0=c(X0), P0 = SigmaX0, dt = B%*%g, Tt = A, ct=0, Zt = C, HHt = Sigma1, GGt = Sigma2, yt = matrix(y,nrow=1))

# Plotting one-step predictions with the given parameters
plot(y, xlab="Frame", ylab="height [m]")
with(kf1, matlines((at[1,]) + cbind(0,-1.96*sqrt(Pt[1,1,]),1.96*sqrt(Pt[1,1,])),type="l", lty=c(1,2,2), col=2))
lines(ypred, col=3)

# Plotting the estimated velocity
with(kf1, matplot((at[2,]) + cbind(0,-1.96*sqrt(Pt[2,2,]),1.96*sqrt(Pt[2,2,])),type="l", lty=c(1,2,2), col=2))


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

## ----------------------------------------------------------------
# Add noise
## ----------------------------------------------------------------
sd <- 1
ynew <- y + rnorm(length(y), sd=sd)
# Then also set the variances again
(Sigma1 <- ??)
(Sigma2 <- ??)
# Estimate and predict again, try different noise level and different variances
# Use the Kalman filter from the FKF package
kf <- fkf(a0=c(X0), P0 = SigmaX0, dt = B%*%g, Tt = A, ct=0, Zt = C, HHt = Sigma1, GGt = Sigma2, yt = matrix(ynew,nrow=1))
# Plot
plot(ynew, xlab="Frame", ylab="height [m]")
matlines((kf$at[1,]) + cbind(0,-1.96*sqrt(kf$Pt[1,1,]),1.96*sqrt(kf$Pt[1,1,])), type="l", lty=c(1,2,2), col=2)


## ----------------------------------------------------------------
# Add a step to y
## ----------------------------------------------------------------
sd <- 1
step <- c(rep(0,20), rep(10,n-20))
ynew <- y + rnorm(length(y), sd=sd) + step
# Then also set the variances again
(Sigma1 <- ??)
(Sigma2 <- ??)
# Estimate and predict again
# Estimate and predict again, try different noise level and different variances
# Use the Kalman filter from the FKF package
kf <- fkf(a0=c(X0), P0 = SigmaX0, dt = B%*%g, Tt = A, ct=0, Zt = C, HHt = Sigma1, GGt = Sigma2, yt = matrix(ynew,nrow=1))
plot(ynew, xlab="Frame", ylab="height [m]")
matlines((kf$at[1,]) + cbind(0,-1.96*sqrt(kf$Pt[1,1,]),1.96*sqrt(kf$Pt[1,1,])), type="l", lty=c(1,2,2), col=2)
matlines((kf$att[1,]) + cbind(0,-1.96*sqrt(kf$Ptt[1,1,]),1.96*sqrt(kf$Ptt[1,1,])), type="l", lty=c(1,2,2), col=3)


## ----------------------------------------------------------------
# Estimate the g
## ----------------------------------------------------------------
# Set the constant matrices and vectors of the Kalman filter
# Extend the state to include the acceleration and add that to the speed
# Transition matrix
(A <- matrix(c(??
               ??
               ??), nrow=3, byrow=TRUE))
# Input matrix is now 0
# Output matrix
(C <- matrix(c(1,0,0), nrow=1))

# State covariance
(Sigma1 <- diag(c( 0.05^2, (0.05/fps)^2, 1)))
# Output variance
(Sigma2 <- matrix(0.1^2))
# Input matrix
(B <- matrix(0, nrow=3, ncol=1))
# Initial values
# The initial state values
(X0 <- c(2.1, y[2] - y[1], 0)) # [m , m/f, m/f^2]
# Initial state covariance
(SigmaX0 <- diag(c( 0.5^2, (1/fps)^2, (1/fps)^2)))

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

# Estimate and predict again
# Use the Kalman filter from the FKF package
kf <- fkf(a0=X0, P0 = SigmaX0, dt = B, Tt = A, ct=0, Zt = C, HHt = Sigma1, GGt = Sigma2, yt = matrix(y,nrow=1))
plot(y, xlab="Frame", ylab="height [m]")
matlines((kf$at[1,]) + cbind(0,-1.96*sqrt(kf$Pt[1,1,]),1.96*sqrt(kf$Pt[1,1,])), type="l", lty=c(1,2,2), col=2)
matlines((kf$att[1,]) + cbind(0,-1.96*sqrt(kf$Ptt[1,1,]),1.96*sqrt(kf$Ptt[1,1,])), type="l", lty=c(1,2,2), col=3)

# The estmated acceleration
plot(kf$att[3,])
abline(h=g, col=3)

# The estimated speed
with(kf, matplot((at[2,]) + cbind(0,-1.96*sqrt(Pt[2,2,]),1.96*sqrt(Pt[2,2,])),type="l", lty=c(1,2,2), col=2))