2.160 System Identification, Estimation, and Learning. Lecture Notes No. 8. March 6, 2006

Transcription

1 2.160 Sysem Idenificaion, Esimaion, and Learning Lecure Noes No. 8 March 6, Eended Kalman Filer In many pracical problems, he process dynamics are nonlinear. w Process Dynamics v y u Model (Linearized) Kalman Gain & Covariance Updae + _ y ˆ If he process is nonlinear bu smooh, is linearized approimaion may be used for he process model. Eension o non-linear sysem using linearizaion Consider a non-linear, coninuous sysem = f (, u, ) + w ( ) n dim (87) y = h (, )+ v ( ) l dim (88) f (.), and h(.) : known bu non-linear, differeniable funcions u : inpu (deerminisic forcing erm; assumed zero) w, v : uncorrelaed process and measuremen noises E [ w )] = 0, E v )] = 0 E [ ( [ ( 0 s 0 s Q = s Q = s w () w (s)]= E [ v () v (s)]= () v E [ w (s)] = 0 he original Kalman filer is no applicable o his class of non-linear sysems. 1

2 Approach Linearize he non-linear sysem around he sae ha is eiher: 1) Pre-deermined linearized Kalman filer e.g. a rajecory o rack. or 2) Esimaed in real-ime using on-line measuremen Eended Kalman Filer Linearized Kalman Filer Sae Acual Absence of Process Noise: w = 0 rajecory Desired raj. Process Nominal rajecory, Discrepancy ime Feedback conrol o keep rack of he desired rajecory ()= a nominal rajecory in he sae space saisfying he noise-less sae equaion: ()= ( ( ), absence of process noise f ) (89) Consider derivaion ( ) ( ) = ( ) + ( ) (90) ( ) = ( ) + ( ) (91) aylor epansion where ( )= + ) f f (, )+ = f, f (, (92) 2

3 f 1 f 1 f n f 2 f (93) n n = = 1 R Jacobian f f n n 1 n = Combining (91) and (92) + = f (, ) + f + from (89) w ( ) f = + w ( ) Now replacing by and Similarly, from (88), f by F ( ) R n n = F ( ) + w ( ) (94) y = H ( ) + v ( ) ( H ) = h R l n (95) y h + y = h (, ) + + v Replacing y by y y H ( ) Noe ha he above linearized sysem (94), (95) wih F ( ) and H ( ) are in he same form as ha of he original Kalman filer: linear ime-varying sysems. 3

4 4.9.2 Eended Kalman Filer. sae Acual ( ) raj. Wih he measuremen of he acual process, he esimaed rajecory is deemed o be more accurae. Use he sae ˆ( ) esimaed in ( ) Esimaed ˆ( ) Nominal (predeermined) real-ime for linearizing he dynamics he Eended Kalman filer f ˆ, = ˆ = F ( ) ime h = ˆ = H ( ˆ, ) (96) Namely, marices F and H are evaluaed a = ˆ, he esimaed values of he sae in real ime, raher han is nominal values. Noe ha F ( ˆ, ) and H ( ˆ, ) can no be pre-compued in off-line. For esimaing he oupu, however, we do no have o use he linearized model; he nonlinear oupu funcion, eq.(88), can be used: y ˆ( ) = h ( ˆ( ), ) (97) Iniial Condiions: P 0 Iniial Condiions: ˆ0 Measuremen y Compue Kalman Gain K ( ) = P ( )H R 1 Updae Sae Esimae wih new measuremen ˆ( ) = F ˆ( ) + K [ y h ( ˆ( ), )] Sae Esimae ˆ( ) Riccai Differenial Eq. 1 P = FP + PF PH R HP + GQG Updae he linearized model: F = f h, H = = ˆ = ˆ Eended Kalman Filer A criical issue of his Eended Kalman Filer is insabiliy. As esimaed sae ˆ deviaes from he rue sae, he linearized model becomes inaccurae, which may lead o an even larger error in sae esimaion. Care mus be aken in implemening eended Kalman Filer. 4

5 4.10 Implemenaion Issues Kalman filers have been applied o diverse applicaions since early 60 s. Various implemenaion echniques have also been developed. here are hree known failure scenarios in which Kalman filers do no work well: a) Unobservable or nearly unobservable processes b) Numerical insabiliy c) Blind spo a) Poor observabiliy he above firs issue can be checked wih he well-known observabiliy condiion. An alernaive, more pracical mehod is o eamine he error covariance mari P() or P. If he process is poorly observable, he variance associaed wih some unobservable sae variables blow up. If he process is poorly observable, one should change he sensors, or a new sensor mus be added. b) Numerical insabiliy Asymmeric covariance: By definiion, covariance marices are symmery, bu numerically hey may become asymmeric, leading o divergence in recursive compuaion. Suchan asymmeric covariance ofen comes from he compuaion of: P = (I K H )P 1 n n where K R and H R are no square marices. Some round off errors yield an asymmeric poseriori covariance P, alhough he a priori covariance P symmery. o resolve his problem, i is efficien o use Joseph s form (40): P = ( I KH ) P (I KH ) + KR K 1 which is equivalen o (41), as discussed in Secion Noe ha boh erms on he righ hand side are symmeric marices. U-D Facorizaion: Since he covariance mari is a real, symmeric, posiive-definie mari, i can be decomposed o he following U-D Facorizaion form: P = UDU d (98) D = 0 d 2, U = 0 d n where mari D is a diagonal mari while mari U is an upper riangular mari. (41) 1 is 5

6 his paricular form assures he posiive definieness of he covariance mari, and implicily preserves he symmery of P. Furhermore, if he covariance updae formula of Kalman filer is convered o he one in he diagonalized space using he upper riangular mari U, he dynamic range of compuaion reduces o 50 % of he original formulaion. See more deails in Secion 9.5 in Brown and Hwang s ebook. c) Blind spo Consider anoher failure scenario: When boh process noise and measuremen noise covariance marices are deemed o be very small, he sae esimaion error-covariance reduces quickly. his is clear from he Riccai differenial equaion (62): 1 P = FP + PF PH R HP + GQG as well as from he covariance propagaion and updae formulae for discree Kalman filer: P +1 = A + G P A G Q, and P = (I K H )P 1. his implies ha he Kalman gain diminishes quickly. Once he Kalman gain diminishes, he subsequen observaions are ignored. In oher words, he Kalman filer is decoupled from he sensors and he real process. his blind spo problem is ofen riggered by numerical round off error in compuing covariance marices as well Relaionship o Wiener Filer Prior o Kalman s work in he early 60 s, N. Wiener made a significan conribuion o he heory of minimum mean-square error filering in he 40 s. You can see his picure and accomplishmens in he display posed a he Infinie Corridor. he able below compares Kalman filer and Wiener filer: Kalman Filer Sae space Non-saionary Coninuous and discree Wiener Filer Frequency domain Saionary Coninuous Since Wiener filer can be seen as a special case of Kalman filer, he fomer can be derived from he laer. Assume a saionary, ime-invariance process, for which he sae esimaion error covariance in Kalman filer reduces o: dp P () P = cons. = 0 d 0 = FP + P F P H R HP + GQG (67) Using his, he Kalman filer reduces o where 1 ˆ = F ˆ + K ( y H ˆ) (98) 6

7 K = P H aking Laplace ransform of (98), R 1 s ˆ ( s ) = F ˆ( s ) K and using (99) yields:. (99) H ˆ( s ) + K y ( s ) ˆ ( s ) = [ si F + P H R H ] P H R y ( s ) (100) he above epression is equivalen o Wiener filer. 7