Steady-state DKF. M. Sami Fadali Professor EE UNR

Transcription

1 Steady-state DKF M. Sami Fadali Professor EE UNR 1

2 Outline Stability of linear estimators. Lyapunov equation. Uniform exponential stability. Steady-state behavior of Lyapunov equation. Riccati equation. Steady-state behavior of Riccati equation. 2

3 Time-varying Recursion x k + 1 = φ k x + k, x + k = x k + K k φ k = φ k + 1, k z k H k x k Eliminate x + k (or eliminate x k ). x k + 1 = φ k I n K k H k x k + φ k K k z k = Aҧ k x k + φ k Kz k Aҧ k = φ k I n K k H k, φ k = φ k + 1, k x + k + 1 = I n K k + 1 H k + 1 φ k x + k +K k + 1 z k + 1 3

4 Stability of Predictor/Corrector Consider LTI case with a constant gain K. x k + 1 = φ I n KH x k + φkz k x + k + 1 = I n KH φ x + k + Kz k + 1 Stable dynamics for eigenvalues of state matrix inside the unit circle. Example: φ = 0.2I 2, K = T, H = 1 5 Eigenvalues (0.2, 0.14): Stable filter. 4

5 Implications of Stability Test Must have a known constant gain to test stability: Kalman filter must be first designed before its stability can be determined. Result is for any gain not necessarily the optimal Kalman gain. Tells us nothing about error dynamics. 5

6 ҧ ҧ Discrete Lyapunov Equation P k+1 = φ k P + k φ T k + Q k Substitute for P + k (Joseph form) P k+1 = φ k I n K k H k P k I n K k H T T k φ k +φ k K k R k K T k φ T k + Q k Lyapunov Equation: P k+1 = Aҧ k P k A T k + തQ k A k = φ k I n K k H k തQ k = φ k K k R k K T k φ T k + Q k Applies for any gain K (not just the optimal Kalman gain K) 6

7 ҧ ҧ ҧ Solution of Lyapunov Eqn. P k+1 = Aҧ k P k A T k + തQ k P k = Φ k, 0 P 0 Φ T k, 0 k 1 + i=0 A k 1 Φ k, i + 1 തQ i Φ T k, i + 1 Φ k, i = A k 2 Aҧ i, For constant A, ҧ Φ k, i = Proof by induction. Φ k, k = I n Aҧ k i 7

8 Uniform Exponential Stability (UES) There exists a positive constant γ > 0, and a constant λ, 0 < λ < 1, s.t. x k γλ k k 0 x k Upper bound Norm of response Response bounded above by an exponential decay curve. 8

9 U.E.S. Theorem x k γλ k k 0 x k 0 Φ k, k 0 i γλ k k 0 Proof of only k, k 0, k k 0 x k = Φ k, k 0 x k 0 Using norm inequalities and the condition x k Φ k, k 0 i x k 0 γλ k k 0 x k 0 Note that the bound on the state-transition matrix implies that Lim Φ k, 0 i is zero. k 9

10 Proof of Necessity Assume uniformly exponentially stable: There exists a positive constant γ > 0, and a constant λ, 0 < λ < 1, s.t. x k γλ k k 0 x k 0 For any k 0, there is a state x a, k a k 0, such that x a = 1, Φ k, k 0 x a = Φ k, k 0 i Initial state x 0 = x a x k a = Φ k, k 0 x a = Φ k, k 0 i Φ k, k 0 i γλ k k 0 k, k 0, k k 0 10

11 U.E.S Condition Theorem: The linear system x k = Φ k, k 0 x k 0 is u.e.s. iff there exist a finite positive constant β s. t. Φ k, k 0 i β, k 0 0 k=k 0 11

12 Proof (Necessity) If the system is U.E.S. k=k 0 Φ k, k 0 i γλ k k 0, k k 0, k 0 Φ k, k 0 i k=k 0 = γ 1 λ = β γλ k k 0 = k=0 γλ k 12

13 ҧ Lyapunov Eqn. Steady-state Solution P k+1 = Aҧ k P k A T k + തQ k, തQ k i < β Q, k Steady-state Solution P = Lim k P k = Lim k k 1 Φ k, 0 P 0 Φ T k, 0 + i=0 First term depends on Lim k Φ k, 0 Φ k, i + 1 തQ i Φ T k, i + 1 i 13

14 Steady-state Solution: UES Assuming u.e.s. x k = Φ k, k 0 x k 0 Lim Φ k, 0 i = 0, k P = Lim k i=0 P i i=0 k 1 k=k 0 Φ k, k 0 Φ k, i + 1 തQ i Φ T k, i + 1 i β Φ k, i + 1 i തQ k i Φ T k, i + 1 i < β 2 β Q <, തQ k i < β Q, k 14

15 ҧ LTI Case Aҧ k A = φ I n KH Φ k, 0 = n i i=1 n ҧ A k = i=1 Z i λ i k Z i λ i k γ λ max k, k 0 λ max = spectral radius of the matrix ҧ A Exponentially stable if and only if λ max < 1 15

16 ҧ ҧ ҧ Exponential Stability Exponential stability for eigenvalues of state matrix inside the unit circle λ max < 1. Aҧ k γ λ i max k, k 0 Exponential stability iff the solution of the algebraic Lyapunov equation is positive definite symmetric. AP A T P = തQ, P = i=0 Aҧ i തQ( Aҧ T ) i A = φ I n KH, തQ = φkrk T φ T + Q > 0 16

17 MATLAB: DLYAP MATLAB solution of algebraic Lyapunov equation >> phi=[0.1,0.2;.3,.4];h=[1,1] ;% observable >>k=[5;-3] ; A= phi*(eye(2)-k*h); >> P=dlyap(A,eye(2)) % Pos. Def. P, Q=I ans = >> eig(phi*(eye(2)-k*h)) % Fast stable dynamics ans =

18 Discrete Riccati Equation P k+1 = φ k P k P k H k T H k P k H k T + R k 1 Hk P k φ k T Derived earlier Nonlinear difference equation. +Q k Valid for the optimal Kalman gain only. 18

19 Algebraic Riccati Equation Consider a time-invariant system with stationary noise. Assume that the limiting solution to the Riccati equation exists. Lim k P k = P P = φ P P H T HP H T + R 1 HP φ T + Q 19

20 Theorem (Lewis, p. 100) If φ, H is detectable (observable) then for every initial matrix P 0 there is a bounded limiting solution to the Riccati equation. The solution is the positive semidefinite (definite) solution of the algebraic Riccati equation. 20

21 MATLAB: DARE (see Kalman) MATLAB solution of algebraic Riccati equation >> phi=[0.1,0.2;.3,.4];h=[1,1]; % Observable >> Q=eye(2) ; R=0.1; >> [X,L,G]=dare(phi,H,Q, R) % P=X, K=G, L=eig(phi*(I-K*H)) X = L = G =

22 Theorem (Lewis, p. 101) Assume that Q = Q 1/2 Q 1/2 T 0, R > 0 and φ, Q 1/2 is reachable. Then H, φ is detectable if and only if 1. There is a unique positive definite limiting solution to the algebraic Riccati equation. 2. The steady-state error system ҧ A = φ I n KH with steady-state Kalman gain K = P H T HP H T + R 1 is asymptotically stable. 22

23 Stability Test Check 1. Q = Q 1/2 Q 1/2 T 0, R > 0 2. φ, Q 1/2 is reachable. 3. H, φ is detectable Apply the last theorem to test the stability of the DKF before you design it. Book: Scalar system with one measurement is reachable and observable (trivial case). 23

24 Example: MATLAB >> phi=[0.1,0.2;.3,.4];h=[1,1]; % For Q= eye(2), Q 1/2 = eye(2) >0 ; R=0.1 >0 ; >> Q= ; eig(q) % Positive eigenvalues for pos. def. >> Qs=sqrt(Q); >> rank(ctrb(phi,qs)) % Check reachability ans = 2 >> rank(obsv(phi,h)) % Check observability ans = 2 24

25 Conclusion Suboptimal filter steady-state behavior: Lyapunov equation. Optimal filter steady-state behavior: Riccati equation. Exponential stability. Can test Kalman filter stability before it is designed and implemented. 25

26 References F. L. Lewis, Optimal Estimation: With an Introduction to Stochastic Control Theory, Wiley-Interscience, New York, W. J. Rugh, Linear System Theory, Prentice- Hall, Upper Saddle River, NJ,