Steady State Kalman Filter

by allenlu2007

Time Varying or Stationary?

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

另一類的應用: 如果 A, B, Bw, C, (參考 Berkeley ME notes) 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

NewImage

NewImage

NewImage

 

 

Another Berkeley article

NewImage

NewImage

Ms 是 prediction step 之後的 covariance.  

NewImage

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]

p = y; sigma = x;  p_dot = z

q0 = a;  q1 = b;  q2 = c

y is solved below (using on-line equation solver).

 

 

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

 

 

 

  Use the on-line equation solver.

NewImage

NewImage

Equation c*(y+R)*(y+2*R)*(y+2*R)=(y*y+b*y-a*y+b*R-a*R)^2 solved for y
Advertisements