Linear-Optimal Estimation for Continuous-Time Systems Robert Stengel Optimal Control and Estimation MAE 546 Princeton University, 2018

Transcription

1 Linear-Opimal Esimaion for Coninuous-Time Sysems Rober Sengel Opimal Conrol and Esimaion MAE 546 Princeon Universiy, 2018! Propagaion of uncerainy in coninuousime sysems! Linear-opimal Gaussian esimaor (Kalman-Bucy filer)! Linear-opimal predicion! Asympoic sabiliy of he sae esimae! Dualiy beween esimaion and conrol! Filer divergence! Square roo filering! Correlaed disurbance and measuremen noise Copyrigh 2018 by Rober Sengel. All righs reserved. For educaional use only. hp:// hp:// 1 Uncerain Linear, Time-Varying (LTV) Dynamic Model Coninuous-ime LTV model wih known coefficiens!x() = F()x() + G()u() + L()w(), x( o ) given x() = x( o ) + [ F(τ )x(τ ) + G(τ )u(τ ) + L(τ )w(τ )]dτ y() = H x x() + H u u() o dim w = s 1 dim z = r 1 Iniial condiion and disurbance inpus are no known precisely Measuremen of sae is ransformed and is subjec o error 2

2 Coninuous-Time Sae Esimaion for LTV Model Same general srucure as discree-ime esimaor Disurbance and measuremen error are Gaussian Sae esimae is Gaussian 3 Propagaion of Mean Value Esimae in Coninuous-Time Sysems E x( 0) = m 0 ; E x E w = 0; E u = m; E x = u By subsiuion in he sysem equaion, he expeced mean value is E!x() Assumed mean value saisics m = F()m + G()u() [ ] = E[ F()x() + G()u() + L()w() ] = F()E[ x() ]+ G()u() = m 4

3 Alernaive Derivaion of Mean Value Esimae Propagaion The sampled-daa case m( k ) = Φ( k k 1 )m( k 1 ) + Φ k τ k k k 1 G τ u(τ ) d Φ( Δ) = I n + FΔ + 1 2! F2 Δ ! F3 Δ 3 + For small Δ m( k ) = ( I n + F k 1 Δ)m( k 1 ) + I n + F k 1 ( k τ ) G( τ )u(τ ) dτ k 1 5 Mean Value Esimae Propagaion m( k ) m( k 1 ) Δ As Δ 0, m( lim k ) m( k 1 ) Δ 0 Δ Rearranging erms Δ = F k 1 m( k 1 ) + I n + F k 1 2 = dm d k 1 k, and G u k 1 k 1 In he limi, he difference equaion converges o a differenial equaion Propagaion of he expeced sae = m = F()m + G()u() 6

4 Covariance Esimae Propagaion Assumed covariance saisics E x ( ) m ( T { ) x( τ ) m( τ ) } = P( ) E w( )w T ( τ ) = Q' C δ( τ ) E u ( ) u ( T { ) u( τ ) u( τ ) } = 0 For small Δ P k = ( I n + F k 1 Δ )P k 1 I n + F k 1 Δ = P k 1 + F k 1 P k 1 Δ + F k 1 P k 1 T + Q k 1 T Δ + F k 1 P k 1 F T k 1 Δ 2 + Q k 1 7 Covariance Esimae Propagaion Rearranging P k P k 1 Δ = F k 1 P k 1 + F k 1 P k 1 T + F k 1 P k 1 F T k 1 Δ + Q k 1 Δ Covariance rae of change As Δ 0, P lim k P k 1 Δ 0 Δ k 1 k = dp d = P Q k 1 = Φ k,τ Q k 1 Δ Disurbance uncerainy k k 1 L τ Q' C L T ( τ )Φ T ( k,τ )dτ L ( )Q' C L T ( )Δ, as k 1 k L ( )Q' C ( ) L T ( ) P = FP + PF T + LQ' C L T 8

5 Kalman-Bucy Filer 9 Kalman-Bucy* Filer Opimal esimaor for linear sysems wih Gaussian uncerainy Three equaions 1) Sae esimae exrapolaion and updae 2) Covariance esimae exrapolaion and updae 3) Filer gain compuaion * Rudolf Kalman, RIAS, and Richard C. Bucy, Johns Hopkins Applied Physics Laboraory, Relaed developmens were made by Weiner (1949), Bode and Shannon (1950), Robbins and Munro (1951), Kiefer and Wolfowiz (1952), Blum(1958), Zadeh and Ragazzini (1952), Carlon and Follin (1956), Berkson (1956), Swerling (1959), Kolmogorov(1962), and Yaglom (1962) 10

6 Covariance Esimae (Par 1) P k Subsiue T ( ) = Φ k 1 P k 1 ( + )Φ k 1 + Q k 1 P k P k + In = P 1 k ( ) + H T k R 1 k H k To obain he rae of change of he prior covariance esimae 1 ( ) P k 1 ( ) T = F k 1 P k 1 ( ) + P k 1 ( )F k 1 + Q k 1 Δ F k 1 K k 1 H k 1 P k 1 ( ) K k 1 H k 1 P k 1 K k 1 Δ Δ T F k 1 H k 1 P k 1 ( ) 11 Disurbance and Gain Marices Disurbance specral densiy marix Q k 1 Δ Q k 1 LQ' C L T Δ LQ' C L T Δ 0 Gain marix T K k 1 = P k 1 ( + )H k 1 1 R k 1 Measuremen error covariance marix R k 1 Δ Δ /2 Δ /2 R C δ ( k 1 τ )dτ R k 1 1 Δ Δ /2 R C δ ( k 1 τ )dτ = R C k 1 Δ Δ /2 1 R k 1 = R 1 C ( k 1 )Δ 12

7 P k ( ) P k 1 Δ Covariance Esimae (Par 2) = F k 1 P k 1 ( )+ P k 1 T K k 1 = P k 1 ( + )H k 1 R 1 C Δ F T k 1 F k 1 P k 1 ( + )H T k 1 R 1 C k 1 + Q k 1 Δ H k 1 P k 1 P k 1 ( + )H T k 1 R 1 C H k 1 P k 1 ( ) + P k 1 ( + )H T k 1 R 1 C ( k 1 )H k 1 P k 1 T F k 1 Δ (+) and ( ) values coalesce as Δ 0 propagaion and updae become concurren P k 1 + Δ 0 P k 1 ( ) = P k 1 13 lim Δ 0 P k Covariance Esimae (Par 3) ( ) P k 1 Δ = P! ( ) = F( )P( ) + P( )F T ( ) + L( )Q' C ( )L T ( ) P( )H T ( )R 1 C ( )H( )P( ) The coninuous-ime filer gain marix is K C = PH T R 1 C P = FP + PF T + LQ' C L T K C HP 14

8 Developmen of Coninuous-Time Sae Esimae Combine propagaion and updae equaions ˆx k + ˆx k ( ) = Φ k 1 ˆx k 1 ( + ) + Γ k 1 u k 1 I n + F( k 1 )Δ ˆx k G k 1 Δ u k 1 ˆx k ( + ) = ˆx k ( ) + K k z k H k ˆx k ( ) = I n + F( k 1 )Δ ˆx k 1 ( + ) + G( k 1 )Δ u( k 1 ) +K k { z k H k I n + F( k 1 )Δ ˆx ( + ) + G( k 1 k 1 )Δ u( k 1 )} 15 Coninuous-Time Sae Esimaor In he limi, he difference equaion converges o a differenial equaion As Δ 0, ˆx( lim k ) ˆx( k 1 ) Δ 0 Δ k 1 k ˆx( k 1 ) ˆx( k ) ˆx Sae esimaor dˆx d ˆ x = F()ˆx + G()u() + K C z H ˆx Residual acs as a driving erm in he sae esimae equaion 16

9 Summary of he Kalman-Bucy Filer ˆ x = F()ˆx + G()u() + K C z H ˆx K C Sae Esimaor Filer Gain Marix = PH T R 1 C P = FP + PF T + L P( 0) given Covariance Esimaor Q' C L T K C HP, 17 Block Diagram of he Kalman-Bucy Filer 18

10 Block Diagram of he Covariance Esimae and Gain Compuaion for he Kalman-Bucy Filer 19 Second-Order Example 20

11 Second-Order Example of Kalman Filer Rolling moion of an airplane, coninuous-ime Δ p Δ φ = L p 0 Δp 1 0 Δφ + L δ A 0 Sae esimaor Δδ A + L p 0 Δp w Δˆ p Δ ˆ φ = L p Δˆp Δ ˆφ + L δ A 0 Δδ A + k 11 k 21 k 12 k 22 Filer gain marix Δp M Δφ M h 11 h 12 h 21 h 22 Δˆp Δ ˆφ k 11 k 21 k 12 k 22 = p 11 p 21 p 12 p 22 h 11 h 21 h 12 h 22 T r r Second-Order Example of Kalman Filer!p 11!p 21!p 12 ( )!p 22 ( ) p 12 ( ) = L p 0 p Covariance p 21 ( ) p 22 ( ) esimaor p 11 ( ) p 12 ( ) L + p 0 p 21 ( ) p 22 ( ) 1 0 k 11 k 21 ( ) k 12 ( ) k 22 ( ) h 11 h 21 T + ( ) h 12 ( ) h 22 ( ) 2 L p σ pw p 11 p 21 ( ) p 12 ( ) p 22 ( ) 22

12 Linear-Opimal Predicor k : Curren ime, sec K : Fuure ime, sec Sae esimae exrapolaion (or propagaion) ˆ!x = F ˆx+ G u, ˆx( k ) from Kalman-Bucy filer K [ ] ˆx( K ) = ˆx( k )+ F(τ )ˆx(τ )+ G(τ )Δu(τ ) dτ k Covariance esimae exrapolaion (or propagaion)!p ( ) = F( )P( ) + P( )F T ( ) + L( )Q' C L T ( ), P( k ) from Kalman-Bucy filer P( K ) = P( k ) + K k F( τ )P( τ ) + P( τ )F T ( τ ) + L( τ )Q' C ( τ )L T ( τ ) dτ 23 Discree-Time Linear-Opimal Predicion, u = 0, 100 poins 24

13 Discree-Time Linear-Opimal Predicion, u = 0.02 sin k/2π, 100 poins 25 Dualiy Beween Esimaion and Conrol 26

14 Dualiy Beween Linear-Opimal Esimaion and Conrol Linear-Gaussian Esimaor ˆ x = F()ˆx + G()u() + K C z H ˆx K C = PH T R 1 C P = FP + PF T + LQ' C L T PH T R 1 C HP, P( 0) given Linear-Quadraic Conroller x() = F()x() G()C()x C() = R 1 ()G T ()S S = SF() F() T S Q + SG()R 1 ()G T ()S, S( f ) given 27 Dual Marix Definiions for Soluion of he Riccai Equaion Linear-Quadraic Conroller Conroller Riccai equaion propagaes backward in ime F() G() Q R S S f C Linear-Gaussian Esimaor F T () H T LQ' C L T R C P P 0 K C T Esimaor Riccai equaion propagaes forward in ime 28

15 Sabiliy of he Kalman-Bucy Filer 29 Sochasic Equilibrium of he Consan-Gain Kalman-Bucy Filer For linear, ime-invarian sysems, covariance esimae approaches a non-zero seady sae P = FP + PF T + LQ' C L T K C HP Δ 0 0 Seady-sae covariance marix is he posiive-definie soluion o an algebraic Riccai equaion FP SS + P SS F T + LQ' C L T K C HP SS = 0 Corresponding filer gain is consan K C = P SS H T R C 1 30

16 Asympoic Sabiliy of he Consan-Gain Kalman-Bucy Filer Esimaion error ε = x ˆx k ; ε = x xk ˆ Linear, ime-invarian sae esimaor = Fˆx + Gu() + K C z Hˆx = ( F K C H) ˆx + Gu() + K C z ˆ!x Consan filer gain marix K C = P SS H T R C 1 Algebraic Riccai equaion FP SS + P SS F T + LQ' C L T K C HP SS = 0 31 Asympoic Sabiliy of he Seady- Sae Kalman-Bucy Filer LTI sae equaion x = Fx + Gu() + Lw Measuremen equaion = Hx + n z!ε ( ) =!x ( ) ˆ! xk ( ) = Fx( ) + Gu + Lw( ) Esimaion error propagaion { + Gu + K C z( ) Hˆx ( ) } Fˆx ε = ( F KH)ε + Lw K C n Esimaor sabiliy governed by (F KH) Esimaor response driven by w() and n() 32

17 I I Informaion Marix Informaion marix and is ime rae of change = P 1 I = P 1 P = P 1 P 1 = I P I Subsiue informaion marix in marix Riccai equaion = I F + F T I + I LQ' C L T I 0 = I SS F + F T I SS + I SS LQ' C L T I SS H T R C 1 H H T R C 1 H Seady-sae soluion for he informaion marix 33 Lyapunov Funcion for Esimaor Error Lyapunov funcion for esimaion error V ε = ( εt ) I SS ε Time rae of change of he Lyapunov funcion mus be negaive o assure sabiliy dv ε d Homogeneous error dynamics ε = ( F KH)ε = 2ε T ( ) I SS!ε ( ) = ε T + ( I SS F H T R 1 C H) T I SS F H T R C 1 H I SS LQ C 'L T I SS + H T R C 1 H = ε T ε T < 0 εt ( ) I SS LQ C 'L T I SS + H T R 1 C H is posiive definie 34

18 Eigenvalues of he Consan-Gain Kalman-Bucy Filer LTI sae esimaion error ε = ( F KH)ε + Lw K C n Sabiliy indicaed by eigenvalues of (F KH) si n ( F KH) = Δ esimaor ( s) = 0 By dualiy o he LQ regulaor, sabiliy of he esimae is guaraneed if he model is correc and ( F,H) : Deecable pair ( F,D) : Sabilizable pair, where LWL T = D T D LWL T : Posiive semi-definie marix R C : Posiive definie marix 35 Sabiliy of he Esimae vs. Sabiliy of he Sysem Esimae error is sable even if he sysem is no!x = ax, a = ±1, x( 0) = 1 = 0.5 (sable), = 2 (unsable) z = x ˆx 0 q = r = 1 36

19 Filer Divergence Could he sae esimae diverge from is mos likely mean value? Yes, if here are Discrepancies in he dynamic model, e.g., nonlinear sysem wih linear model Errors in he assumed covariance marices Measuremen biases Numerical errors in calculaion RMS error = roo-mean-square error = esimaed sandard deviaion RSS error = roo-of-sum-ofsquare errors = square roo of he empirical sum of he squares of he difference beween acual and esimaed sae componens 37 Examples of Filer Divergence (Schlee, Sandish, Toda, 1967) Divergence due o Modeling Error Divergence due o Compuaional Error Saellie aliude esimaion using simplified 1-D model Filer designed assuming consan aliude Drag and graviaional effecs change as aliude increases or decreases 38

20 Examples of Filer Divergence (Schlee, Sandish, Toda, 1967) Telescope Landmark Tracker Effec of Model Deail on Filer Divergence Orbi deerminaion for a space vehicle Measuremens are angle sighings of known erresrial landmarks Increased model precision can reduce divergence rae 6 - sae filer : Spacecraf posiion & velociy 9 - sae filer : Spacecraf posiion & velociy, Landmark posiion 39 Landmark and Sar Tracking Landmark and sar racking are funcionally similar Insrumen has narrow field of view Sar/geographic locaion caalogs help idenify arges x and y locaion of landmark or sar on focal plane deermines angles o he arge 40

21 Divergence Occurs When Filer Gains are Too Low 1) Dynamic model is incorrec 2) Sae-error covariance esimae is incorrec 3) Filer gains become oo low o properly weigh new informaion 41 Soluions for Filer Divergence Increase process noise assumed for esimaor design process noise = assumed disurbance covariance Improve sysem modeling, e.g., Esimae measuremen bias and scale facor Include higher-order erms Model nonlineariy Use higher precision or square-roo filering Adap esimaor o changing condiions 42

22 Recall Effecs of Assumed Process Noise and Measuremen Error Q = R = 1 Q = 0.01 R = 1 Q = 0.01 R = Adding Process Noise o Eliminae Divergence* Saellie orbi deerminaion Aerodynamic drag produced unmodeled bias Opimal filer did no esimae bias Process noise increased for filer design Divergence was conained Filer s esimae of RMS sae error Acual sae esimae error Filer s esimae of RMS sae error Acual sae esimae error * Fizgerald,

23 Coninuous-Time Square Roo Filering 45 Square-Roo Filering Improved precision by reducing condiion number Define P = SS T where : Lower riangular square roo of P [see ex] S Use Cholesky decomposiion o compue S Riccai equaion P = FP + PF T + LQ' C L T PH T R 1 C HP!S ( )S T ( ) + S( ) S! T ( ) = Subsiue = F( )S( )S T ( ) + S( )S T ( )F T ( ) + L( )Q' C ( )L T S( )S T ( )H T ( )R 1 C ( )H( )S( )S T ( ) 46

24 Marix Decomposiion Examples Cholesky Decomposiion UDU T (or LDL T ) Decomposiion Examples from Wikipedia hp://en.wikipedia.org/wiki/cholesky_decomposiion 47 Square-Roo Filering Pre-muliply by S 1 ; pos-muliply by S T S 1!S +!S T S T = S 1 FS + S T F T S T + S 1 LQ' C L T S T S T H T R C 1 HS " M = M LT + M UT Elemens of M LT () ( m ij ) LT = m ij, i > j m ij 2, i = j 0, i < j M LT M UT M UT : Lower riangular porion of M : Upper riangular porion of M T = M LT Because S is lower riangular S and S 1 are lower riangular 48

25 K C Square-Roo Filering Hence, he Riccai equaion becomes S = SM LT, S( 0)S T ( 0) = P( 0) > 0 Esimaor gain marix = SS T H T R 1 C, S( 0)S T ( 0) = P( 0) > 0 Cholesky decomposiion required o define S(0) and M LT () 49 Linear-Opimal Filer for Correlaed Disurbance Inpus and Measuremen Noise* Nose Boom Measuremens!P Disurbance process produces error in measuremens Correlaion expressed by expeced value = F w E n w( τ ) n( τ ) = Q C M C T Riccai equaion Esimaor gain marix M C R C P( ) + P( )F T ( ) + L( )Q' C ( )L T ( ) P( )H T ( ) + L( )M C ( ) R 1 C ( ) H( )P K C * Bryson and Johansen δ τ + M C T = PH T + LM C R 1 C L T ( ) 50

26 Nex Time: Nonlinear Sae Esimaion 51 Supplemenal Maerial 52

27 Program for Example of Kalman Filer Esimae Error Sabiliy % Kalman-Bucy Filer Esimae Sabiliy % Copyrigh by Rober Sengel. All righs reserved. % 4/11/2010 clear % Firs-Order Sable and Unsable Sysems SableSys = ss(-1,1,1,0); UnsableSys = ss(1,1,1,0); [Ksable,Lsable,Psable] = kalman(sablesys,1,1,0); [Kunsable,Lunsable,Punsable] = kalman(unsablesys,1,1,0); SableSysEs = ss((-1-lsable),1,1,0); UnsableSysEs = ss((1-lunsable),1,1,0); % Response = [0:0.01:6]; [y1,1,x1] = iniial(sablesys,1,); [y2,2,x2] = iniial(unsablesys,1,); [y3,3,x3] = iniial(sablesyses,0.5,); [y4,4,x4] = iniial(unsablesyses,-1,); figure y3 = inerp1(3,y3,1); y4 = inerp1(4,y4,2); plo(1,y1,2,y2,1,(y1-y3),2,(y2-y4)),grid, axis([ ]) 53