Lecture Note 12: Kalman Filter

Transcription

1 ECE 645: Estimation Theory Spring 2015 Instructor: Prof. Stanley H. Chan Lecture Note 12: Kalman Filter LaTeX prepared by Stylianos Chatzidakis) May 4, 2015 This lecture note is based on ECE 645Spring 2015) by Prof. Stanley H. Chan in the School of Electrical and Computer Engineering at Purdue University. 1 Introduction Kalman filter is a set of mathematical equations proposed by Rudolf E. Kálmán in 1960 for estimating the future, present and past states of a process. It provides a recursive formula which, coupled with the recent advances in digital systems and communications, allows for a powerful way to track/predict/forecast dynamical systems using current estimates and observations. Kalman filter has important applications in signal processing, tracking, and navigation. In this lecture, we will study some basic concepts of Kalman filter using the discrete-time model. 2 State-Space Model Definition 1. State-Space Model Let t = 1,2,... be a sequence of discrete time instants. A discrete-time state-space model has the following form: X t+1 = F t X t +G t U t, 1) Y t = H t X t +V t, 2) where X t R n is the tth state, U t R n is the input, F t R n n and G t R n n are linear mappings, Y t R m is the tth observation, V R m is the noise, and H t R m n is another linear mapping. The following is an example showing the state-space model of a mechanical model: Example 1. Let P t be the position of an object at time t, V t be its velocity and A t be its acceleration. Suppose that we observe P t at t = T s,2t s,...,nt s. We like to derive the state-space model. To derive the state-space model, we first note that P n+1)ts = P nts +T s V nts, V n+1)ts = V nts +T s A nts.

2 Therefore, the state-space equation becomes ) Pn+1)Ts 1 Ts = 0 1 V n+1)ts ) PnTs V nts P nts = 1 0 ) P nts V nts ). ) ) 0 + A T n T s, s The general problem of Kalman filter is that suppose we have observed Y 0,...,Y t, how should we estimate the tth state X t? To answer this question, we first introduce a notation Definition 2. We denote for any a, b. Y b a def = {Y a,...,y b }, Therefore, if we use the MMSE estimation method, the estimation problem can be formulated as [ def X t = argmin E Xt,Y Xt t Y t 0 o) X t ) 2], 3) X t where X t is a function of the observation Y t 0 and X t has some prior distribution. By Bayesian MMSE estimator in Lecture 9, we have that X t y t 0) = E Xt,Y t 0 [ Xt Y t 0 = y t ] 0. 4) So now the question is: Support that X t satisfies the state space model, how do we compute the conditional expectation? 3 Kalman-Bucy Filter 3.1 Main Results Assumption 1. We assume that U t N0,Q t ) is iid Gaussian distribution V t N0,R t ) is iid Gaussian distribution X 0 is iid Gaussian distribution We are interested in deriving the following quantities: X t t = E[X t+1 Y t 0], which is the estimate of the next state based on the current observations, and X t+1 t = E[X t+1 Y t 0], 2

3 which is the estimate of the current state based on the current observations. These two quantities can be estimated through the following sequence of lemmas. Lemma 1. By definition, we know that X t+1 t = E[X t+1 Y t 0] X t+1 t = F t Xt t. 5) = E[F t X t +G t U t Y t 0 ] = F t E[X t Y t 0 ]+G te[u t Y t 0 ] = F t Xt t +G t E[U t Y t 0]. We claim that E[U t Y t 0 ] = 0. To see this, we observe that Y t = H t X t +V t = H t F t 1 X t 1 +G t 1 U t 1 )+V t. Since Y t depends only on U 0,...,U t 1, it must be independent of U t. Therefore E[U t Y t 0 ] = E[U t] = 0, and hence X t+1 t = F t Xt t. Lemma 2. where K t = Σ t t 1 H T t X t t = X t t 1 +K t Y t H t Xt t 1 ), 6) Ht Σ t t 1 H T t +R t ) 1. Since the output equation is given by by using the MMSE estimator we have that X t t = E[X t Y t 0] = E[X t Y t,y t 1 0 ] = E[X t Y t 1 0 ]+CovX t Y t 1 0 )H T t Y t = H t X t +V t, Ht CovX t Y t 1 0 )H T t +R t ) 1Y t H t E[X t Y t 1 0 ]). Defining Σ t t 1 = CovX t Y t 1 0 ), and usingthe fact that X t t 1 = E[X t Y t 1 0 ], the above equation can be simplified as X t t = X t t 1 +Σ t t 1 H T t H tσ t t 1 H T t +R t) 1 Y t H t Xt t 1 ). 3

4 If we further define K t = Σ t t 1 H T t Ht Σ t t 1 H T t +R ) 1, t then X t t becomes X t t = X t t 1 +K t Y t H t Xt t 1 ). Lemma 3. Σ t+1 t = F t Σ t t F T t +G t Q t G T t. 7) Recall that Since Σ t+1 t def = CovX t+1 Y t 0 ). X t+1 = F t X t +G t U t, we have CovX t+1 Y t 0) = CovF t X t +G t U t Y t 0) = F t CovX t Y t 0)F T t +G t CovU t Y t 0)G T t = F t Σ t t F T t +G t Q t G T t. where Σ t t = CovX t Y t 0 ) and CovU t Y t 0 ) = CovU t) = Q t. Therefore, Σ t+1 t = F t Σ t t F T t +G t Q t G T t. Lemma 4. Σt t = Σ t t 1 Σ t t 1 H T t H t Σ t t 1 H T t +R t ) 1 HΣ t t 1. 8) By the MMSE result, we know that for a linear system Y = HΘ+V, the conditional covariance is Σ = Σ θ Σ θ H T HΣ θ H T +Σ) 1 HΣ θ. Using this result by considering the state space equation Y t = H t X t +V t, we have Σ t t def = CovX t Y t 0) = CovX t Y t 1 0,Y t ) = CovX t Y t 1 0 ) CovX t Y t 1 0 )H T t H t CovX t Y t 1 0 )H T +R t ) 1 H t CovX t Y t 1 0 ) = Σ t t 1 Σ t t 1 H T t H t Σ t t 1 H T t +R t ) 1 HΣ t t 1. 4

5 3.2 Structure of Kalman Filter To summarize the structure of a Kalman-Bucy Filter, we put the above four lemmas together as X t t = X t t 1 +K t Y t H t Xt t 1 ), 9) Σ t t = Σ t t 1 K t H t Σ t t 1, 10) X t+1 t = F t Xt t, 11) Σ t+1 t = F t Σ t t F T t +G t Q t G T t. 12) This set of four equations determine the structure of the Kalman filter. A schematic diagram is shown below. X t t + + Y t + K t + F t Xt+1 t + H t Xt t 1 delay Figure 1: Schematic diagram of a Kalman filter. 3.3 Analysis of Residue Definition 3. Define the residue as and define We are interested in analyzing the difference Theorem 1. I t is a zero mean Gaussian with CovI t,i s ) = 0 if t s. I t = Y t H t Xt t 1, 13) Ŷ t t 1 = H t Xt t 1. 14) I t = Y t Ŷ t t 1. 15) E[I t ] = E[Y t Ŷt t 1] = E[Y t E[Y t Y t 1 0 ]] = E[Y t ] E[E[Y t Y t 1 0 ]] = E[Y t ] E[Y t ] = 0. 5

6 where in a) we used the fact that CovI t,i s ) = E[I t,i T s] = E[E[I t I T s Y s 0]] a) = E[E[I t Y s 0]I T s] = 0, E[I t Y s 0] = E[Y t Y s 0] E[E[Y t Y t 1 0 ] Y s 0] = E[Y t Y s 0] E[Y t Y s 0] = Example Consider a scalar Kalman Filter: X t+1 = fx t +U t, Y t = hx t +V t, wheref,h are scalars and t = 0,1,... We would like to derive the recursion formula for this Kalman Filter, where U t N0,q), V t N0,r). Solution 1. By using the Kalman filter equations, we observe that where the Kalman gain constant is The conditional covariances are X t+1 t = f X t t X t t = X t t 1 +K t Y t h X t t 1 ), K t = Σ t t 1 hhσ t t 1 h+r) 1 = Σ t t 1h Σ t t 1 +r. Σ t+1 t = fσ t t f +1q1 = f 2 Σ t t +q. Σ t t = Σ t t 1 Σ t t 1 hhσ t t 1 h+r) 1 hσ t t 1 = Σ t t 1 h2 Σ 2 t t 1 Σ t t 1 +r = Σ t t 1r Σ t t 1 +r. 6

7 It is worth investigating the behavior of Σ t+1 t as t approaches infinity. Suppose that Σ t+1 t approaches to a constant Σ, then Σ must satisfy: Σ t+1 t = f2 Σ t t 1 r Σ +q t t 1 Σ = f2 Σ r Σ +q. The above equation is quadratic in Σ. Therefore, Also, Noted that Σ = 1 2 { r 1 f2 ) q) 2 + 4rq 1 2} r 21 f2 )+q. 16) Σt+1 t Σ = f 2 Σ t t 1 r Σ t t 1 +1 x ax+1 y ay +1 = axy +x axy y ax+1)ay +1) x y = ax+1 ay +1 x y, Σ r Σ ) where the inequality holds because a > 0, x > 0, y > 0, ax+1 1 and ay As a result, Σ t+1 t Σ f 2 Σ t t 1 Σ f 2t+1) Σ 0 Σ. Therefore, if f < 1 then Σ t+1 t approaches Σ as t. 7