Implementing a 1-D Kalman Filter Regression, Missing the smoothing action (getting the opposite)

Question

I am implementing the 1D Kalman Filter in Python on a fundamentally noisy set of measurement data, and I should be observing a large amount of smoothing...but, instead, my Kalman Filter is doing the exact opposite of smoothing, so that my filtered results look like this:

And since my "model" is just the simple trend formula, $$x_{k} = 3*(x_{k-1} - x_{k-2}) + x_{k-3} $$ the model comes up with outrageous predictions when the expected smoothing doesn't happen:

I am working off an incompletely specified 1D Kalman Filter implementation, so I am having to fill in the gaps...but I should be seeing much softer filtered values, with the peaks and valleys being outliers, with my naive model. Also, my predictions should actually be more modest than the actual data (which would follow from a smoothed filtered collection).

I am trying to figure out what I am missing. Here is the pseudo code for what I have implemented so far:

 Step 1: Minimize T = -log(Q/R) in order to discover Q
         Here I basically run the kalman filter with given T values, 
         calculating Q from the Residual std

 Step 2: Run Kalman Filter with T held constant

 xprior = [x1,x2,x3] (calculated from the first three values of data)

 #prediction
 x_{k|k-1} = model_estimate(xprior)

 #add measurement
 innovation = data[k] - x_{k|k-1}
 K = R / (R + Q)

 #filtered value
 x_{k|k} = x_{k|k-1} + K*(innovation)


 xprior <- update_xprior(x_{k|k})

 residuals <- update_residuals(measurement,x_{k|k})

 R = variance(residuals)
 Q = e^(-T)*R

 ...

I must be missing something...Also, I am not confident that what I am doing with T and the way I am calculating Q is correct...but I should be seeing smoothing regardless, since that is what they did in the document I am working off of.

Answer 1

So let's try to piece the signal model together first.

The state vector, $\mathbf{x}_k$ is just $$ \mathbf{x}_k = [ x_{k-1}\ x_{k-2}\ x_{k-3} ]^T $$ and the state update equation is just $$ \mathbf{x}_k = \left[ \begin{array}{ccc} 3 & -3 & 1 \\ 1 & 0 & 0 \\ 0 & 1 & 0 \\ \end{array} \right] \mathbf{x}_{k-1} + \left[ \begin{array}{c} 1 \\ 0 \\ 0 \\ \end{array} \right] v_k $$ where $v_k$ is some driving noise which is Gaussian, zero mean, with variance $\sigma_v^2$.

Then the output equation is just $$ x_k = \left[\begin{array}{ccc} 1 & 0 & 0 \end{array} \right] \mathbf{x}_k + w_k $$ where $w_k$ is Gaussian, zero mean, with variance $\sigma_w^2$.

If I set $\sigma_v$ to be $1$ and $\sigma_w$ to be 0.01 and generate one realization of the signal then I get the plot below (blue line)

This seems to be similar to the measurements you're graphing.

And the red line is what happens when I try to apply the Kalman filter to the signal model above.

Bottom line: I seem to get similar results to what you are seeing.

I suspect part of the problem is that the signal model is not stable, as the state transition matrix has eigenvalues:

1.0000055+0.0000000i 0.9999973+0.0000048i 0.9999973-0.0000048i

the first of which is definitely outside the unit circle.

---

R Code Below

#27879

T <- 100

#H = [3 -3 1]
H <- matrix(c(1,0,0),c(1,3)) 

#F is just a tapped delay line
alpha <- matrix(c(0.9,0,0,0,0.9,0,0,0,0.9),3,3)
A <- matrix(c(3,1,0,-3,0,1,1,0,0),3,3)
B <- matrix(c(1,0,0),c(3,1))

sigma_v <- 1
v <- rnorm(T,0,sigma_v)
sigma_w <- 0.01
w <- rnorm(T,0,sigma_w)

x <- matrix(c(0,0,0),c(3,1))
z <- rep(0,T)

for (t in 1:T)
{
  z[t] <- H %*% x + w[t]
  x <- A %*% x +v[t]
}
par(mfrow=c(1,1))
plot(z,type="l", col="blue", lwd=5)

Q <- matrix(c(0,0,0,0,0,0,0,0,sigma_v^2),3,3)
R <- sigma_w^2


library("MASS") # For pseudo inverse ginv()

xkm1km1 <- matrix(rep(0,3*T+3),3,T+1)
xkkm1 <- matrix(rep(0,3*T),3,T)
K <- matrix(rep(0,3*T),3,T)
Pkm1km1 <- matrix(c(1000,0,0 ,0,1000,0, 0,0,1000),3,3)
zhat <- matrix(rep(0,T),c(T,1))

for (k in 1:T)
{
  xkkm1[,k] <- A %*% xkm1km1[,k]
  Pkkm1 <- A %*% Pkm1km1 %*% t(A) + Q
  K[,k] <- Pkkm1 %*% t(H) %*% ginv( H %*% Pkkm1 %*% t(H) + R)
  xkm1km1[,k+1] <- xkkm1[,k] + K[,k] %*% (z[k] - H %*% xkkm1[,k])
  Pkm1km1 <- (matrix(c(1,0,0,0,1,0,0,0,1),3,3) - K[,k] %*% H) %*% Pkkm1  
  zhat[k] <- as.numeric(H %*% xkkm1[,k])
}
lines(zhat, col="red")