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

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]

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.

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