16.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 1 of 7 Lecture 24 Last time: () () () () () () () () TT d X t AtXt XtAt BtNtBt dt =+ + If the system reaches a statistical steady state, the covariance matrix would be constant. The system would have to be invariant, () 0 TT d 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 () 0Xt > , () 0Xt> for all t. If, in addition, [ ] ,AC is observable, then Cov ( ) 0yt??> ?? for all .t Kalman Filter Our text treats two forms of the Kalman filter: ? Discrete time filter Based on the model o System: ()1() kk x kxkwφ+= + o Measurements: () kk k zHxkv=+ ? Continuous time filter o System: x Fx Gw=+& o Measurements: zHxv=+ 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 Page 2 of 7 The augmented system is modeled as () () ()x Atx Gtu Btn=++& where ()nt is an unbiased white noise with correlation function ()() ( ) () T Entn 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 () kkkk zHxt v=+ where k v is an unbiased, independent noise sequence with T kk k E 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 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 ()Pt in time starting from ( ) 1 ? k x t + ? and () 1k Pt + ? to ( )? k x t ? and () k Pt ? 2. Incorporate the measurement at k t to produce ( )? k x t + and () k Pt + Time Propagation After processing the measurement at 1k t ? we have the estimate () 1 ? k x t + ? and the estimation error covariance matrix ( ) 1k Pt + ? . Then the filter operates open loop until the next measurement point at k t . Note that we are not considering any effects of computation delays here. In the interval from 1k t ? to k t the system dynamics are described as () () ()x Atx Gtu Btn=++& 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 Page 4 of 7 () () 11 ??() () ?? kk x Atx Gtu xt x t + ?? =+ = & The error dynamics are then ?() () () ? () () et xt xt exx Ate Btn =? =? =+ & && The mean of the error then satisfies ()eAte= & since () 0nt= , and since the initial condition is () 1 0 k et ? = we have () 0et= , () 1 , kk tt +? ? 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 TT PAPPA BNB=+ + & with the initial condition () () 11kk Pt P t + ?? = So the time propagation step can be taken by integrating the differential equations () ( ) () () 11 11 ?? ?? kk TT kk xt x t xAxGu Pt P tPAPPA BNB + ?? + ?? = =+ ==+ + & & 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 () () 11 ,(), kk d tt At tt dt ?? Φ=Φ with the initial condition () 11 , kk 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 Page 5 of 7 () () 1 11 ??( ) , ( ) , ()() k k t kkkk k t x tttxt tGudτ τττ ? ?+ ?? =Φ + Φ ∫ Note the simplification if ()u τ is constant over this interval. The corresponding covariance matrix would be () () () () 1 11 1 () , ( ) , , () ()() , k k t TT T kkkkk 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 ?? kkkkkk kkkkk T kkkkk x xGuw xxGu PPQ φ φ φφ + ?+ + ?+ + =++ =+ =+ where T kkk Qww= If you wanted to express continuous time dynamics in discrete time form, you could only do it if ()u τ were constant ( ) k u= in ( ) 1 , kk tt + . Then () () () () () 1 1 1 1 1 1 11 , ,() ,()() ,()()() , k k k k k k kkk t kk t t kk t T kkk t T T kk t tt GtGd wtBnd Qww tBNB t d φ τττ ττττ τ τττ ττ + + + + + + ++ =Φ =Φ =Φ = =Φ Φ ∫ ∫ ∫ Measurement Update At the next measurement point, k t , we have the a priori estimate, ? () k x t ? , its error covariance matrix, () k Pt ? , and a new measurement of the form () kkkk zHxt v=+ where the noises, k v , are an unbiased, independent sequence of random variables with covariance matrix, k R . 16.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 6 of 7 T kk k Evv R??= ?? 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. 12 ??x Kx Kz +? =+ We wish to choose 1 K and 2 K so as to: 1. Preserve the estimation error unbiased 2. Minimize the a posteriori error variance The a posteriori error is 12 ? ? exx x Kx Kz ++ ? =? =? ? ()() () 12 1 2 exKxe KHxv I KKHxKeKv +? ? =? ? ? + =?? + The mean a posteriori error is (remember that the a priori error and the measurement noise are zero-mean) () 12 eIKKHx + =?? We want this to be zero for all x . Therefore 12 KIKH=? With this choice of 1 K , 0e + = and () () 22 2 ?? ?? x IKHx Kz xKzHx +? ?? =? + =+ ? 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 2 K that minimizes the a posteriori error variance. 16.322 Stochastic Estimation and Control, Fall 2004 Prof. Vander Velde Page 7 of 7 () () 2 2 ?? ? ? x xKHxvHx xKHev +? ? ?? =+ +? =+ + () () 2 22 ? ? exx x xKHev I KH e Kv ++ ?? ? =? =? ? + =? Note that v is independent of e ? . ()() ()() 2222 222 T T TTT T T PEee I KH ee I KH KvvK IKHPIKH KRK +++ ?? ? ??= ?? =? ? + =? ? + 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 + ()() 222 Min tr( ) tr tr T T JP I KH P I KH KRK + ? = ?? ? ?=? ? + ? ? ?? We use a derivative form () tr 2 T ABA AB A ? ?? = ?? ? if B is symmetric. ()() ()() {} () () 222 22 22 2 1 2 2 20 T T TT TT J I KH P I KH KR KK IKHP H KR PH K HPH R KPHHPHR ? ? ?? ? ?? ?? =? ? + =? + =? + + = =+ This is known as the Kalman gain.