State Estimation by steady state Kalman Filter

Question

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 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.

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

Answer 1

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.

---

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

Answer 2

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.