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:

enter image description here

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:

enter image description here

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)

 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.


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)

enter image description here

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


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]
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")
| improve this answer | |
  • $\begingroup$ Awesome. Yeah, this is the first time I did the K-filter, so I wasn't sure I was doing it incorrectly... This is making me scratch my head, because apparently the 'model' used to create the graphs in the paper isn't the same one they published in text... $\endgroup$ – Chris Dec 24 '15 at 13:13
  • $\begingroup$ BTW, does w[t] in the line z[t] <- H %*% x + w[t] add a random value from the gaussian to the estimate?) Otherwise, I am not sure how "adding" the gaussian works... $\endgroup$ – Chris Dec 24 '15 at 15:37
  • $\begingroup$ Yes, the w[t] is adding the measurement noise to the output. All the w values are set in the line w <- rnorm(T,0,sigma_w). Can you point to the paper? I'd be interested to see what they're trying to do. That system seems strange, as I said, because it's unstable, and the KF generally doesn't work well with unstable systems. $\endgroup$ – Peter K. Dec 24 '15 at 16:01
  • $\begingroup$ @PeterK., are you sure about computing Pkkm1? It seems to me that $x_{k-1}, x_{k-2}, x_{k-3}$ are noisy and as a result the uncertainty in $x_{k}$ should be affected by the uncertainties in $x_{k-1}, x_{k-2}, x_{k-3}$. It seems kind of an accumulated uncertainty. $\endgroup$ – CroCo Jan 19 '16 at 17:38
  • $\begingroup$ @CroCo Pkkm1 is always data independent in the KF. That is, it doesn't depend on any value of $x_k$ (or $x_{k-1, k-2, \ldots, k-m}$). $\endgroup$ – Peter K. Jan 19 '16 at 17:44

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.