EE226a - Summary of Lecture 13 and 14 Kalman Filter: Convergence

Transcription

1 1 EE226a - Summary of Lecture 13 and 14 Kalman Filter: Convergence Jean Walrand I. SUMMARY Here are the key ideas and results of this important topic. Section II reviews Kalman Filter. A system is observable if its state can be determined from its outputs (after some delay). A system is reachable if there are inputs to drive it to any state. We explore the evolution of the covariance in a linear system in Section IV. The error covariance of a Kalman Filter is bounded if the system is observable. The covariance increases if it starts from zero. If a system is reachable and observable, then everything converges and the stationary filter is asymptotically optimal. II. REVIEW: KALMAN FILTER We consider a linear system with matrices that do not depend on time. That is, X n+1 = AX n + V n and Y n = CX n + W n, n 1, where {X 1, V n, W n, n 1} are all orthogonal and zero-mean with cov(v n ) = K V and cov(w n ) = K W. Theorem 1: Kalman Filter ˆX n = L[X n Y 1,..., Y n ] = L[X n Y n ] is obtained by the Kalman Filter: ˆX n = A ˆX + R n (Y n CA ˆX ) where R n = S n C T [CS n C T + K W ] 1, S n = AΣ A T + K V, Σ n = (I R n C)S n. Thus, S n+1 = K V + AS n A T AS n C T [CS n C T + K W ] 1 CS n A T. Moreover, the matrices S n and Σ n have the following significance: Proof: Let S n = cov(x n A ˆX ), Σ n = cov(x n ˆX n ). U n = Y n L[Y n Y ] = Y n L[CX n +W n Y ] = Y n CL[X n Y ] = Y n CA ˆX = CX n +W n CL[X n Y ] Then, by (1) in Lecture 12, ˆX n = L[X n Y n ] = L[X n Y ] + L[X n U n ] = A ˆX + R n U n = A ˆX + R n (Y n CA ˆX ) where R n = cov(x n, U n )cov(u n ) 1. Now, cov(x n, U n ) = cov(x n, CX n + W n CL[X n Y ]) = cov(x n L[X n Y ], CX n + W n CL[X n Y ]) = S n C T. Also, cov(u n ) = cov(cx n + W n CL[X n Y ]) = CS n C T + K W.

2 2 Thus, In addition, R n = S n C T [CS n C T + K W ] 1. S n = cov(x n L[X n Y ]) = cov(ax + V A ˆX ) = AΣ A T + K V. Finally, by (1) in Lecture 12, Σ n = cov(x n L[X n Y n ]) = cov(x n L[X n Y ]) cov(x n, U n )cov(u n ) 1 cov(u n, X n ) = S n R n CS n. In the following sections, we explore what happens as n. Specifically, we want to understand if the error covariance Σ n blows up or if it converges to a finite value. First, we recall some key notions about linear systems. III. OBSERVABILITY AND REACHABILITY Lemma 1: Cayley-Hamilton Let A R m m and det(si A) = α 0 + α 1 s + + α m 1 s m 1 + s m. Then α 0 I + α 1 A + + α A m 1 + A m = 0. In particular, span{i, A, A 2,...} = span{i, A,..., A m 1 }. Definition 1: Observability and Reachability The linear system X n+1 = AX n, Y n = CX n, n 1 (1) is observable if X 1 can be determined exactly from {Y n, n 1}. We then say (A, C) is observable. The linear system X n+1 = AX n + CU n, n 1 (2) is reachable if, for every state X, there is a sequence of inputs {U n, n 1} that drives the system from X 1 = 0 to state X. We say that (A, C) is reachable. Fact 1: (a) (A, C) is observable if and only if [C T A T C T (A m 1 ) T C T ] is of full rank. (b) (A, C) is reachable if and only if (A T, C T ) is observable, i.e., if and only if [C AC A m 1 C] is of full rank. In that case, m 1 A p CC T (A p ) T is positive definite. Proof: To see (a) note that (1) implies that Y n = CX n = CA X 1. Consequently, observability is equivalent to the null space of [C CA CA 2 ] being {0}. Accordingly, this is equivalent to the matrix [C T A T C T (A 2 ) T C T ] being of full rank. The conclusion then follows from Lemma 1. For (b), observe that (2) implies that X n = A X 1 + A n k 1 CU k = A n k 1 CU k. Therefore, the system is reachable if and only if [C AC A 2 C ] is of full rank. The conclusion then follows again from Lemma 1. IV. SYSTEM ASYMPTOTICS Our discussion is borrowed from [2]. First, let us examine the evolution of the unobserved system X n+1 = AX n + V n, n 1, where {X 1, V n, n 1} are all orthogonal and zero-mean with cov(v n ) = K V and K n = cov(x n ). Note that K n+1 = AK n A T + K V. (3) The following theorem describes the evolution of K n. Recall that a matrix A R m m is said to be stable if its eigenvalues all have magnitude strictly less than 1. Theorem 2: (a) If A is stable, then there is a positive semidefinite matrix K such that K n K as n. Moreover, K is the unique solution of the equation K = AKA T + K V. (4)

3 3 (b) Let K V = QQ T. Assume that (A, Q) is reachable. Then A is stable if and only if the equation has a positive definite solution K. Proof: (a) By induction we see that the solution of (3) is K = AKA T + QQ T n 2 K n = A K 1 (A ) T + A p K V (A p ) T. (5) If A is stable, then (A p ) i,j Cα p for some finite C and some α (0, 1), as it can be seen by considering the SVD of A. This implies that the first term in (5) vanishes and the sum converges to some K = Ap K V (A p ) T that is clearly positive semidefinite. The convergence also implies (4). It remains to show that (4) has a unique solution. Let K be another solution. Then = K K satisfies = A A T. Recursive substitutions show that = A n (A n ) T. Letting n shows that = 0 and the solution of (4) is unique. (b) If A is stable, we know from (a) that (4) has a unique positive semidefinite solution K = Ap QQ T (A p ) T. Since (A, Q) is reachable, the null space of [Q T Q T A T Q T (A 2 ) T ] is {0}. This implies K is positive definite. To show the converse, assume that K is a positive definite solution of (4). Then we know that K = A n K(A n ) T + Ap QQ T (A p ) T, n 1. Let λ be an eigenvalue of A T with eigenvector v. Then A T v = λv and ( ) v Kv = λ 2n v Kv + v A p QQ T (A p ) T v. But Ap QQ T (A p ) T is positive definite from the reachability of (A, Q), which implies that the last term in the above identity is positive. Consequently, it must be that λ < 1. The statements of the theorem should be intuitive. If the system is stable, then the state tries to go to 0 but is constantly pushed by the noise. That noise cannot push the state very far and one can expect the variance of the state to remain bounded. The convergence is a little bit more subtle. If the system is reachable, then the noise pushes the state in all directions and it is not surprising that the variance of the state is positive definite if the system is stable. If the system is not stable, the variance would explode. V. FILTER ASYMPTOTICS We now explore the evolution of Kalman Filter. Theorem 3: Let K V = QQ T. Suppose that (A, Q) is reachable and (A, C) is observable. If S 1 = 0, then The limiting matrices are the only solutions of the equations Σ n Σ, R n R, S n S, as n. Σ = (I RC)S, R = SC T (CSC T + K W ) 1, and S = AΣA T + K V. Equivalently, S is the unique positive semidefinite solution of S = A(S SC T ( CSC T + K W ) 1 CS)A T + K V. (6) Moreover, the time-invariant filter Z n = AZ + R(Y n CAZ ) satisfies cov(z n Ẑn) Σ, as n. Comments: The reachability implies that the noise excites all the components of the state. The observability condition guarantees that the observations track all the components of the state and imply that the estimation error remains bounded. Note that the state could grow unbounded, if A is unstable, but the estimator tracks it even in that case. The time-invariant filter has the same asymptotic error as the time-varying one. Proof: The proof has the following steps. For two positive semidefinite matrices S and S, we say that S S if S S is positive semidefinite. Similarly, we say that the positive semidefinite matrices {S n, n 1} are bounded if S n S for some positive semidefinite matrix S. (a) The matrices S n are bounded. (b) If S 1 = 0, then S n is nondecreasing in S 1. (c) If S 1 = 0, then S n S, where S is a positive semidefinite matrix. (d) The matrix A ARC is stable. (e) For any S 1, S n S. (f) Equation (6) has a unique positive semidefinite solution S. (g) The time-invariant filter has the same asymptotic error covariance as the time-varying filter. We outline these steps.

4 4 (a) The idea is that, because of observability, X n+m is a linear function of {Y p, V p, W p, p = n,..., n + m 1}. The covariance S n+m must be bounded by that of X n+m given {Y p, p = n,..., n + m 1}, which is a linear combination of the covariances of 2m random variables and is therefore uniformly bounded for all n. (b) That is, if S 1 is replaced by S 1 S 1, then S n is replaced by S n S n. The proof of this fact is pretty neat. One could try to do it by induction, based on the algebra. That turns out to be tricky. Instead, consider the following argument. Since we worry only about covariances, we may assume that all the random variables are jointly Gaussian. In that case, increasing S 1 to S 1 > S 1 can be done by replacing X 1 by X 1 = X 1 + ξ 1, where ξ 1 = N(0, S 1 S 1 ) and is independent of {X 1, V n, W n, n 0}. Now, let X n be the system state corresponding to X 1 and Y n be the corresponding observation. Note that X n+1 = X n+1 + A n ξ 1. Consequently, and while L[X n+1 Y n, ξ 1 ] = L[X n+1 Y n ] + L[X n+1 ξ 1 ] + L[A n ξ 1 Y n ] + L[A n ξ 1 ξ 1 ] = L[X n+1 Y n ] + A n ξ 1 S n+1 = cov(x n+1 L[X n+1 Y n, ξ 1 ]) S n+1 = cov(x n+1 L[X n+1 (Y ) n ]). Since, for each n 1, one can express Y n as a linear function of (Y n, ξ 1 ), it follows that S n+1 S n+1. (c) Note that S 1 = 0 S 2. From part (b), S n S n+1. However, S n B, for some positive semidefinite B. Thus, S n (i, i) := e T i S ne i S n+1 (i, i) B(i, i). This implies that S n (i, i) S(i, i) for some nonnegative S(i, i). Similarly, α(n) := S n (i, i) + 2S n (i, j) + S n (j, j) = (e i + e j ) T S n (e i + e j ) α(n + 1) (e i + e j ) T B(e i + e j ). This implies that α(n) α. We conclude that S n (i, j) must converge to some S(i, j). Hence S n (i, j) S(i, j). (d) Simple algebraic manipulations show that S = (A ARC)S(A ARC) T + ARK W R T A T + K V. Assume that (A ARC) T v = λv with λ > 1 and nonzero v. Then v Sv = v (A ARC)S(A ARC) T v + v ARK W R T A T v + v K V v = λ 2 v Sv + v ARK W R T A T v + v K V v. Since λ > 1, it is obtained that v Sv = v ARK W R T A T v = v K V v = v QQ T v = 0, which implies Qv = 0 and (A, Q) is not reachable. (e) We borrow this proof from [3]. The notation is the same as before. Consider the system ξ n+1 = A T ξ n + C T ζ n + W n. The problem is to find the ζ k (ξ k ) for k = 1,..., n to achieve Φ n (ξ) = min E[ {ξk T K V ξ k + ζk T K W ζ k } + ξn T S 1 ξ n ξ 1 = ξ]. By considering all possible choices for ζ 1 and assuming that the cost is minimized over {2,..., n + 1}, one sees that A direct calculation (see below) allows to verify that and the minimizing values of ζ n are given by Instead of using this optimal control, one uses In that case, the cost is ξ T G n ξ where Φ n+1 (ξ) = min ζ {ξ T K V ξ + ζ T K W ζ + E(Φ n (A T ξ + C T ζ + W 1 ))}. Φ n (ξ) = ξ T S n ξ + trace (S k K V ) ζ n k = [CS k C T + K W ] 1 CS k A T ξ n k. ε n k = [CSC T + K W ] 1 CSA T ξ n k. G n+1 = ΓG n Γ T + ARK W R T A T + K V, with G 1 = S 1 and Γ = A ARC. Since Γ is stable from part (d), we see that G n converges to S (Theorem 2). Now, Φ n (ξ) = ξ T S n ξ + trace (S k K V ) ξ T G n ξ, k=0 for all ξ. This implies that S n G n. Recall that if S 1 = 0, then the resulting covariance matrices, S n, are such that S n S. Moreover, we see that S n S n G n with S n S and G n S. It follows that S n S.

5 5 In the calculations above, we used the fact that, if A 1 is symmetric and invertible, then and is achieved by min ζ {ξ T A 0 ξ + ζ T A 1 ζ + 2ξ T A 2 ζ} = ξ T [A 0 A T 2 A 1 1 A 2]ξ ζ = A 1 1 A 2ξ, as a direct calculation shows. (f) Assume that S is another positive semidefinite solution of (6). If S 1 = S, then S n = S. However, we know from (e) that S n S. Hence, S = S. (g) This follows immediately because the time-invariant filter is a special case of the time-varying filter. REFERENCES [1] R. G. Gallager: Stochastic Processes: A Conceptual Approach. August 20, [2] P. R. Kumar and P. Varaiya: Stochastic Systems: Estimation, Identification, and Adaptive Control. Prentice-Hall, [3] H. Kushner: Introduction to Stochastic Control. Holt, Rinehart and Winston, Inc., 1971.