4
$\begingroup$

I am working with a discret Kalman filter on a System

$x_{k+1}=A_k x_k+B_k u_k+\omega_k$

$y_k=C_k x_k+\upsilon_k$

$E[\omega_k\omega_k^T]=Q$

$E[\upsilon_k \upsilon_k^T]=R$

I have estimated the State from the available noisy $y(k)$, which is generated from the same system state equations with Reference Trajectory of the state. Then I have tested it with a wrong initial state $x_0$ and a big initial covariance (simulation 1). I have noticed that the KF works very well, after a few steps the gain $K$ quickly converges to a very small value near zero. I think it is maybe caused by the Process noise $Q$. I have set it small because the $Q$ stands for the accuracy of the model.

Now I want to modify it to a Steady State Kalman filter. I used the steady gain from simulation 1 as constant instead of the calculation in every iteration. Then the five equations can be simplified to one equation:

$\hat{x}(k+1)=(I-KC)A \hat{x}(k)+(I-KC)B u(k)+K y(k+1)$

I want to test it with same initial state and covariance matrix as the one in simulation 1. But the result is very different from reference trajectory, and even the result of simulation 1. I have tested it with the covariance matrix $p_\infty$, which is solved from the discret Riccati equation. And the use the equation:

$k_\infty=\dfrac{p_\infty C'}{C p_\infty C'+R}$

This neither works. I am wondering:

  1. How should I apply Steady State Kalman Filter and how should I set the initial state for it?

  2. Is Steady State KF only used for scalar system?

  3. Should I use it with LQ-controller or some others?

Eddit

I want to compare the performence between standard Kalman filter and steady state Kalman filter like this: firstly, I set a wrong Initial State: $X_0=[5, -6, 0]^T$ but in fact it should be $X_0=[0, 0, 0]^T$. And I set the $P_0$ like a diagonal Matrix, on the main Diagonal are $25, 36, 0$. I get the steady gain $k_\infty$ and $p_\infty$ from solving the discret Riccati equation

$P_\infty=AP_\infty A^T-(AP_\infty C^T)(CP_\infty C^T+R)^{-1}(CP_\infty A^T)+Q$

This can be solved by using Matlab Toolbox. I adjust the Koefficient $Q$ ($R$ is constant and available) until the "real" Kalman gain same as $k_\infty$ and at the same time standard Kalman filter works well. Then I think $Q$ in this time the best one. The result is good enter image description here And now I don't know, how to set the parameter in steady state Kalman filter. I set $X_0=[5, -6, 0]^T$ and $R$ same as ones in the standard kalman filter and use $k_\infty$ as constant Kalman gain. But it works with same parametmer not well. enter image description here

I like to know, what's wrong with my design. The plot is below:

$\endgroup$
3
  • $\begingroup$ Your second equation implies that $R=0$ is that true? If so, your Kalman filter is ill-posed. Also your statement Q stands for the accuracy of the model is not correct, I believe. Generally, $Q$ is the process noise variance, which says something about how energetic variations in the driving system are. Can you clarify? $\endgroup$
    – Peter K.
    Apr 7, 2016 at 16:33
  • $\begingroup$ @ Peter K. No, $R$ is not zero but constant. I thinke, the second equation of steady kalman filter is undependent on the Matrix $R$ because $R$ is in Standard Kalman filter only used for the calculation for $K$ and in SSKF we don't need calculate the gain every step. $\endgroup$
    – wangmars
    Apr 7, 2016 at 17:35
  • $\begingroup$ @Peter K. And I would like to know, how to calculate the $Q$ matrix. Because I use the simulated Measurement, I think $Q$ is diagonal Matrix, and I set it just as a small value. $\endgroup$
    – wangmars
    Apr 7, 2016 at 17:41

2 Answers 2

1
$\begingroup$

I've just tried a toy example of doing what you're suggesting and it works fine. The plot below shows how the standard Kalman filter (red line) and the steady-state Kalman filter (green circles) outputs are the same, except for some minor differences at the beginning.

The bottom plot shows the evolution of the Kalman gain coefficients for the standard Kalman filter.

Without seeing more detail of what you're doing, I don't think I can help much. Note that I'm using SISO example, which may not be what you're after... but I haven't ever seen reasons for MIMO models to work differently than expected.

enter image description here


R Code Below

#27879 and 29963

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(2,1))
plot(z,type="l", col="blue", lwd=5)
title('Measurements and both Filtered Measurements')

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")

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

for (k in 1:T)
{
  xkkm1[,k] <- A %*% xkm1km1[,k]
  K[,T] <- Pkkm1 %*% t(H) %*% ginv( H %*% Pkkm1 %*% t(H) + R)
  xkm1km1[,k+1] <- xkkm1[,k] + K[,T] %*% (z[k] - H %*% xkkm1[,k])
  zhat[k] <- as.numeric(H %*% xkkm1[,k])
}
points(zhat, col="green")

plot(1:100, K[1,], type="l", ylim=c(-1,1))
title('Kalman Gain coefficients')
lines(1:100, K[2,], col="red")
lines(1:100, K[3,], col="green")
$\endgroup$
4
  • $\begingroup$ @ Peter K.♦ Thanks. I'd like to say, that I don't know how to use steady state kalman filter. I have added some details in my question about how have i set the parameter. And I want to konw, why ist the steady state Kalman gain in second graph not constant? $\endgroup$
    – wangmars
    Apr 8, 2016 at 7:40
  • $\begingroup$ I'm a little skeptical about the $R$ matrix being diag(25,36,0). Can you try setting the third entry to something small but non-zero (like 1)? $\endgroup$
    – Peter K.
    Apr 11, 2016 at 20:52
  • $\begingroup$ And without knowing the details of your model and reference, I'm not sure I can help much more. $\endgroup$
    – Peter K.
    Apr 11, 2016 at 20:57
  • $\begingroup$ Hi: in general, when using a kalman filter, the general approach is to use some subset ( or maybe all ) of the data to estimate the two variances: noise variance and state variance. See harvey's 1990 blue text " strucural models and the kalman filter , for how to do that using a likelihood decomposition. Then, given those estimates, you use them as if they were the true values and then run the filter to steady state. Steady state just means that the variance of the state converges to some constant. Nothing really special about it. $\endgroup$
    – mark leeds
    Jan 30, 2018 at 4:39
0
$\begingroup$

When estimating a state whose A, Q, and R matrices are time-invariant, it is computationally efficient to compute the steady-state gain and covariance offline. It is the same result that you would get if you just let the Kalman Filter run to infinity. The problem is probably with your low $Q$ numbers. As some have already stated, $Q$ better represents additive Gaussian noise in the actual trajectory of $x$, rather than model inaccuracies.

That said, starting from a large error $\hat{x}_0 - x$ is equivalent to the true process having had a massive disturbance equal to this error. Since your $Q$ is so small, the filter is not designed to handle such large deviations from $Ax + Bu$. Usually, if you start with a large error, then you should start with a large covariance $P_0$ and use the regular Kalman Filter. You need a representative $\hat{x}_0 - x$ that aligns with the Riccati-calculated $P_0$ if you want to use the steady-state version.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

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