16.322 Stochastic Estimation and Control, Fall 2004 Prof vander velde Lecture 245 Last time dt X()=A(1)X(t)+X()4()y+B()N()B(t)y If the system reaches a statistical steady state, the covariance matrix would be constant. The system would have to be invariant, X(0=AX +XA+BNB=0 and the steady state covariance matrix can be solved for from this algebraic equation Note that: X(t) is symmetric If [A, B] is controllable and X(o)>0, X(()>0 for all If,in addition, [A, c] is observable, then Cov y(o>0 for all t Kalman filter Our text treats two forms of the kalman filter Discrete time filter Based on the model stem:x(k+1=ox(k)+w o Measurements: =k=Hx(k)+V Continuous time filter o System: x= Fx+Gw o Measurements: z=Hx+v The most common practical situation is neither of these. In the aerospace area, and many other application areas as well, we are interested in dynamic systems that evolve continuously in time so we must at least start with a continuous system model. The system may be driven by a known command input and may be subject to random disturbances. If the given disturbances cannot be well approximated as white, then a shaping filter must be added to shape the given Page 1 of 7
16.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 1 of 7 Lecture 24 Last time: () () () () () () () () d T T X t At X t X t At Bt N t Bt dt =+ + If the system reaches a statistical steady state, the covariance matrix would be constant. The system would have to be invariant, () 0 d T T X t AX XA BNB dt =+ + = and the steady state covariance matrix can be solved for from this algebraic equation. Note that: X ( )t is symmetric. If [A,B] is controllable and 0 X t() 0 > , X t() 0 > for all t . If, in addition, [A C, ] is observable, then Cov ( ) 0 ⎡ ⎤ y t > ⎣ ⎦ for all t. Kalman Filter Our text treats two forms of the Kalman filter: • Discrete time filter Based on the model o System: ( ) 1 () k k x k xk w += + φ o Measurements: ( ) kk k z Hxk v = + • Continuous time filter o System: x& = + Fx Gw o Measurements: z Hx v = + The most common practical situation is neither of these. In the aerospace area, and many other application areas as well, we are interested in dynamic systems that evolve continuously in time, so we must at least start with a continuous system model. The system may be driven by a known command input and may be subject to random disturbances. If the given disturbances cannot be well approximated as white, then a shaping filter must be added to shape the given disturbance spectrum from white noise
16.322 Stochastic Estimation and Control, Fall 2004 Prof vander velde Augmented n(t) Disturbance System ( Shaping filter noise Generator disturbance xt) control Inpu The augmented system is modeled as 文=A(1)x+G(t)u+B(1)n where n(t) is an unbiased white noise with correlation function E[n)r)]=N()(-) We allow the system matrices to be time varying. The most common reason for dealing with time varving linear dynamics is because we have linearized nonlinear dynamics about some trajectory But although the system dynamics are continuous in time, we do not process measurements continuously. The Kalman filter involves significant computations, and those computations are universally executed in a digital computer. Since a digital computer works on a cyclic basis, the measurements are processed at discrete points in time The measurements are modeled as Hkx()+V where v is an unbiased, independent noise sequence with E||=R If the measurements have a noise component which is correlated in time, a continuous time shaping filter must be added to the augmented system model designed to give the proper correlation for the noise samples. We assume here that this augmentation, if needed, has been done Although we can allow for a correlated measurement noise component, it is essential that there be an independent noise component as well. If there is no independent noise component, the whole nature of the optimum filter changes o the physical situation of most common interest is a hybrid one Page 2 of 7
16.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 2 of 7 The augmented system is modeled as x& =++ At x Gt u Bt n () () () where n t( ) is an unbiased white noise with correlation function () ( ) () ( ) T E ntn Nt t ⎡ ⎤ τ = − δ τ ⎣ ⎦ We allow the system matrices to be time varying. The most common reason for dealing with time varying linear dynamics is because we have linearized nonlinear dynamics about some trajectory. But although the system dynamics are continuous in time, we do not process measurements continuously. The Kalman filter involves significant computations, and those computations are universally executed in a digital computer. Since a digital computer works on a cyclic basis, the measurements are processed at discrete points in time. The measurements are modeled as ( ) k kk k z Hxt v = + where k v is an unbiased, independent noise sequence with T E kk k ⎡ ⎤ vv R = ⎣ ⎦ If the measurements have a noise component which is correlated in time, a continuous time shaping filter must be added to the augmented system model designed to give the proper correlation for the noise samples. We assume here that this augmentation, if needed, has been done. Although we can allow for a correlated measurement noise component, it is essential that there be an independent noise component as well. If there is no independent noise component, the whole nature of the optimum filter changes. So the physical situation of most common interest is a hybrid one:
16.322 Stochastic Estimation and Control, Fall 2004 Prof vander velde System dynamics are continuous in time Measurements are processed at discrete points in time discrete measurement X(t) D P(t1) k-1 continuous time propagation Where a discrete time measurement is processed there is a discontinuity in the estimate, the estimate error and the error covariance matrix. We use the superscript"-to indicate values before incorporating the measurement at a measurement time and the superscript"+to indicate values after incorporating he measurement at a measurement time The filter operates in a sequence of two-part steps 1. Propagate i(n) and P() in time starting from i'(-)and P*(k-)to i(4) andPˉ 2. Incorporate the measurement at t to produce i'() Time Propagation After processing the measurement at Ik- we have the estimate i'('k-) and the estimation error covariance matrix P*(tk-D) Then the filter operates open loop until the next measurement point at tr. Note that we are not considering any effects of computation delays here In the interval from tk- to tt the system dynamics are described as A(Ox+G(u+b(on The estimate i'(tk-1)is the mean of the distribu measurements processed up to that point. So the estimate error is zero We want to preserve the error unbiased until the next measurement, and we can achieve this driving the estimate with the mean or expectation of the system Page 3 of 7
16.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 3 of 7 • System dynamics are continuous in time • Measurements are processed at discrete points in time Where a discrete time measurement is processed, there is a discontinuity in the estimate, the estimate error, and the error covariance matrix. We use the superscript “-“ to indicate values before incorporating the measurement at a measurement time and the superscript “+” to indicate values after incorporating the measurement at a measurement time. The filter operates in a sequence of two-part steps 1. Propagate xˆ( )t and P t( ) in time starting from xˆ (tk 1 ) + − and P t( ) k 1 + − to xˆ (tk ) − and ( ) P tk − 2. Incorporate the measurement at kt to produce xˆ (tk ) + and P t( ) k + Time Propagation After processing the measurement at k 1 t − we have the estimate ( ) 1 ˆ k x t + − and the estimation error covariance matrix ( ) P tk 1 + − . Then the filter operates open loop until the next measurement point at kt . Note that we are not considering any effects of computation delays here. In the interval from k 1 t − to kt the system dynamics are described as x& =++ At x Gt u Bt n () () () The estimate ( ) 1 ˆ k x t + − is the mean of the distribution of x conditioned on all the measurements processed up to that point. So the estimate error is zero mean. We want to preserve the error unbiased until the next measurement, and we can achieve this driving the estimate with the mean or expectation of the system dynamics
16.322 Stochastic Estimation and Control, Fall 2004 Prof vander velde =A(1)+G(1) ( The error dynamics are then (1)=x(1)-X() A(De+b(o)n The mean of the error then satisfies since n(t)=0, and since the initial condition is (t-)=0 we have e(0=0, (+-p4) The form of the error dynamics are a linear system driven by white noise, which is the form of dynamics we just analyzed. The covariance matrix for the estimation error therefore satisfies the differential equation P=AP+ Pa+BNB with the initial condition P(-)=P() So the time propagation step can be taken by integrating the differentia x= ax+gu (-)=x(-) P=AP+PA+ BNB P(k-1)=P*(4-1) Both of these relations can be expressed in the form of discrete time transitions but especially if A and/ or B and/or G are time-varying, this may not be convenient The transition matrix would be the solution to the differential equation d Φ(t1)=AO)(,1-) with the initial condition d Then the estimate at the next measurement point would be given by Page 4 of 7
16.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 4 of 7 ( ) 1 1 ( ) ˆ ˆ () () ˆ ˆ k k x At x Gt u xt x t + − − = + = & The error dynamics are then () () () ˆ ˆ () () et xt xt e xx At e Bt n = − = − = + & & & The mean of the error then satisfies e At e = ( ) & since n t() 0 = , and since the initial condition is ( ) 1 0 k e t − = we have e t() 0 = , ( ) 1, k k t t + − − The form of the error dynamics are a linear system driven by white noise, which is the form of dynamics we just analyzed. The covariance matrix for the estimation error therefore satisfies the differential equation T T P AP PA BNB & =+ + with the initial condition () () Pt P t k k 1 1 + − − = So the time propagation step can be taken by integrating the differential equations ( ) ( ) () () 1 1 1 1 ˆ ˆ ˆ ˆ k k T T k k x Ax Gu xt x t P AP PA BNB Pt P t + − − + − − = + = =+ + = & & Both of these relations can be expressed in the form of discrete time transitions, but especially if A and/or B and/or G are time-varying, this may not be convenient. The transition matrix would be the solution to the differential equation () () 1 1 , () , k k d tt At tt dt Φ =Φ − − with the initial condition ( ) 1 1 , k k tt I Φ = − − Then the estimate at the next measurement point would be given by
16.322 Stochastic Estimation and Control, Fall 2004 Prof vander Velde x()=Φ(4,-)(4-)+」中(4,)G(r)(r)dr lote the simplification if u(r) is constant over this interval. The corresponding covariance matrix would be P(4)=(24-)P(-)(4,4-)+∫中(2)B(r)N(r)B(r)(t,)dr Both of these expressions are much more complex to deal with than would be the case if one were just given a discrete time model of the system dynamics-such as is done in the text x=Bx +g,u +W 式1=+G P1=D PD+OK where Qr=w,1 If you wanted to express continuous time dynamics in discrete time form, you could only do it if u(r)were constant(=u)in(k, i-1).Then =Φ(t,l) =∫p(tn,x)B(r)n(r)dr Q4= (t,r)B(r)N(r)BxyΦ(t1,r)d Measurement update At the next measurement point, tk, we have the a priori estimate, x (t,), its error covariance matrix, P(tR), and a new measurement of the form where the noises, v, are an unbiased, independent sequence of random variables with covariance matrix R Page 5 of 7
16.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 5 of 7 ( ) ( ) 1 1 1 ˆ ˆ ( ) , ( ) , ()() k k t k kk k k t x t tt xt t G u d τ τ ττ − − + =Φ + Φ − − ∫ Note the simplification if u( ) τ is constant over this interval. The corresponding covariance matrix would be ( ) ( ) () () 1 11 1 ( ) , ( ) , , () () () , k k t T T T k kk k kk k k t Pt tt Pt tt t B N B t d τ τ ττ τ τ − − + =Φ Φ + Φ Φ −− − ∫ Both of these expressions are much more complex to deal with than would be the case if one were just given a discrete time model of the system dynamics – such as is done in the text. 1 1 1 ˆ ˆ k kk kk k k kk kk T k kk k k x x Gu w x x Gu P PQ φ φ φ φ + − + + − + + =+ + = + = + where T Q ww k kk = If you wanted to express continuous time dynamics in discrete time form, you could only do it if u( ) τ were constant (= uk ) in (t t k k , +1 ). Then ( ) ( ) ( ) () () 1 1 1 1 1 1 1 1 , , () , ()() , () () () , k k k k k k k kk t k k t t k k t T k kk t T T k k t t t G t Gd w t Bnd Q ww t BNB t d φ τ ττ τ τ ττ τ τ ττ τ τ + + + + + + + + = Φ = Φ = Φ = =Φ Φ ∫ ∫ ∫ Measurement Update At the next measurement point, kt , we have the a priori estimate, ˆ ( ) k x t − , its error covariance matrix, ( ) P tk − , and a new measurement of the form ( ) k kk k z Hxt v = + where the noises, k v , are an unbiased, independent sequence of random variables with covariance matrix, Rk
16.322 Stochastic Estimation and Control, Fall 2004 Prof vander velde E[]=R We wish to incorporate the information contained in this measurement in our estimate of the state To this end, postulate an updated estimate, I', in the form of a general linear ombination of the a priori estimate and the measurement. For simplicity I will suppress the time arguments x=K1x+K2三 We wish to choose k, and K. so as to: 1. Preserve the estimation error unbiased 2. Minimize the a posteriori error variance The a posteriori error is e=x-x x-K1x-K2三 e*=x-K(x-e)-K2 (Hx+y =(1-K1-K2H)x+Ke-K2 The mean a posteriori error is(remember that the a priori error and the measurement noise are zero-mean) e*=(I-K1-k,H)x We want this to be zero for all x. Therefore K=l-Kh With this choice of k x=(1-K2H)x+K2三 =x+K2(-) o the updated estimate is the a priori estimate plus a gain times the measurement residual. We observed this form earlier when discussing estimation in general ow we wish to find the K, that minimizes the a posteriori error variance Page 6 of7
16.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 6 of 7 T E vv R kk k ⎡ ⎤ = ⎣ ⎦ We wish to incorporate the information contained in this measurement in our estimate of the state. To this end, postulate an updated estimate, xˆ + , in the form of a general linear combination of the a priori estimate and the measurement. For simplicity I will suppress the time arguments. 1 2 xˆ ˆ Kx K z + − = + We wish to choose K1 and K2 so as to: 1. Preserve the estimation error unbiased 2. Minimize the a posteriori error variance The a posteriori error is 1 2 ˆ ˆ e xx x Kx K z + + − = − =− − ( ) ( ) ( ) 1 2 12 1 2 e x K x e K Hx v I K KH x Ke Kv + − − =− − − + =−− + − The mean a posteriori error is (remember that the a priori error and the measurement noise are zero-mean) ( ) 1 2 e I K KH x + =−− We want this to be zero for all x . Therefore K I KH 1 2 = − With this choice of K1 , e 0 + = and ( ) ( ) 2 2 2 ˆ ˆ ˆ ˆ x I KH x Kz x K z Hx + − − − =− + =+ − So the updated estimate is the a priori estimate plus a gain times the measurement residual. We observed this form earlier when discussing estimation in general. Now we wish to find the K2 that minimizes the a posteriori error variance
16.322 Stochastic Estimation and Control, Fall 2004 Prof vander velde x+=x-+ x+1 i+k2(He"+y e=x-x (-K2H)e-K2 Note that y is independent of e P=ELee] (I-K2He-e-(-K2H)+K,vy'K2 (I-K2HP-(I-K2H)+K2RK2 Our optimization criterion will be taken to be the sum of the variances of the a posteriori estimation errors. The sum of the error variances is the trace of P Min J=tr(P) tr[(-K2H)P(l-K H)+t[K, RK] We use a derivative form I tr(ABA)=2AB if B is symmetric =2(-K2H)P (-K2H)+2k2R K2H)P-(-H)+K2 2(PH+K(HPH+R)=0 K2=P-H(HP-HT+R) This is known as the Kalman gain Page 7 of7
16.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 7 of 7 ( ) ( ) 2 2 ˆˆ ˆ ˆ x x K Hx v Hx x K He v +− − − − = + +− =+ + ( ) ( ) 2 2 2 ˆ ˆ e xx x x K He v I KH e Kv + + − − − = − =− − + =− − Note that v is independent of e − . ( ) ( ) ( )( ) 2 2 22 2 2 22 T T TT T T T P E ee I K H e e I K H K vv K I K H P I K H K RK + ++ − − − = ⎡ ⎤ ⎣ ⎦ =− − + =− − + Our optimization criterion will be taken to be the sum of the variances of the a posteriori estimation errors. The sum of the error variances is the trace of P+ ( )( ) 2 2 22 Min tr( ) tr tr T T J P I K H P I K H K RK + − = =− − + ⎡ ⎤ ⎡ ⎤ ⎣ ⎦ ⎣ ⎦ We use a derivative form tr 2 ( ) T ABA AB A ∂ ⎡ ⎤ = ⎣ ⎦ ∂ if B is symmetric. ( ) ( ) { } ( ) ( ) { } ( ) ( ) 2 22 2 2 2 2 2 1 2 2 2 2 2 0 T T T T T T J I KH P I KH KR K K I KH P H KR P H K HP H R K P H HP H R − − − − − − − ∂ ∂ =− − + ∂ ∂ = − −+ =− + + = = + This is known as the Kalman gain