Steady State Kalman Filter

Time Varying or Stationary?

一般而言 Kalman filter 是 time varying 的 filter.  多數 Kalman filter 的應用也是 time varying system, 例如 navigation, gesture recognition, etc.  

另一類的應用: 如果 A, B, Bw, C, (參考 Berkeley article) matrices and (A, C) is observable or detectable; (A, Bw) is controllable or stabilizable. 

同時 covariance matrices Q, R 是 stationary and semi-positive definite.  Kalman filter 有 steady state 的 solution.  

Steady state 的參數包含:

• state mean, x(k) -> x,  or m(k) -> m (only one)
• state covariance 有兩個:  P+ 是 prediction step 之後的 covariance;  P- 是 update step 之後的 covariance.

        P+ > P-;   P+ = A' P- A + Q

       or P- = A'.inv (P+ - Q) A.inv

• Kalman gain (only one)

 

Method A:  Brute force iterative simulation

Method B:  P = P

Method C:  discrete algebraic Ricartti Equation (DARE) steady state equation 如下:

(可以參考 const_v.py code for simulation example)

 

ARE

RE: Riccarti Equation    ARE: (Algebraic Riccarti Equation)

Stanford EE363 note

 

 

Another Berkeley article

Ms 是 prediction step 之後的 covariance.  

Fs 是 steady state 時的 Kalman Gain!!!

 

Example1:  DC offset estimation: A=C=1 ; Bw=1  

P+ = P- +Q

P+ = P+ + Q - P+ ( P+ + R)^-1 P+

P+ + R = P+^2 / Q

P+ = 0.5 * (Q + sqrt(Q^2 + 4QR))  as expected

  

Example 2:  Constant Velocity Model: A = [1, 1; 0, 1] , C =[1, 0],  Bw=1, V=R, W=[qo, q1; q1, q2]

case 1:  W = [q0, 0; 0, 0]  process noise only on position, reduce to example 1??  Q = q0

case 2:  W = ?? process noise only on velocity ->?