Linear-Optimal State Estimation Robert Stengel Optimal Control and Estimation MAE 546 Princeton University, 218 Linear-optimal Gaussian estimator for discrete-time system (Kalman filter) 2 nd -order example Alternative forms of the Kalman filter equations Ill conditioning Correlated inputs and measurement noise ime-correlated measurement noise Copyright 218 by Robert Stengel. All rights reserved. For educational use only. http://www.princeton.edu/~stengel/mae546.html http://www.princeton.edu/~stengel/optconest.html 1 Uncertain Linear, ime-varying (LV) Dynamic Model x = Φ 1 x 1 Γ 1 u 1 Λ 1 w 1 z = x n = s 1 = r 1 dim w dim z Initial condition and disturbance inputs are not nown precisely Measurement of state is transformed and is subject to error 2
Actual System State Estimation Goals Minimize effects of measurement error on nowledge of the state Reconstruct full state from reduced measurement set (r n) Average redundant measurements (r n) to estimate the full state Method Provide optimal balance between measurements and estimates based on the dynamic model alone 3 Linear-Optimal State Estimation Kalman filter is the optimal estimator for discrete-time linear systems with Gaussian uncertainty It has five equations 1) State estimate extrapolation 2) Covariance estimate extrapolation 3) Filter gain computation 4) State estimate update 5) Covariance estimate update Notation ˆx : Estimate at th instant before measurement update : Estimate at th instant after measurement update ˆx 4
Equations of the Kalman Filter 1) State estimate extrapolation (or propagation) ˆx = Φ 1 ˆx 1 ( ) Γ 1 u 1 2) Covariance estimate extrapolation (or propagation) P = Φ 1 P 1 ( )Φ 1 Q 1 5 Equations of the Kalman Filter K = P 3) Filter gain computation P 4) State estimate update ˆx ( ) = ˆx K z ˆx H R 5) Covariance estimate update 1 = P 1 H R 1 P 1 6
Diagram of State Estimate Fig. 4.3-2, OCE [1] [4] 7 Covariance Estimate and Filter Gain Matrix Fig. 4.3-3, OCE [3] [2] [5] 8
Alternative Expressions for K I r = R 1 R Matrix Identity K = P P H R 1 = P H R 1 R P H 1 R K I r P H 1 R = P H 1 R K = P H 1 R P H R 1 1 I r Multiply by inverse K = P H R 1 K P H 1 R = ( I n K )P H 1 R Subtract from both sides Collect terms 9 = P 1 H R 1 P Alternative Expressions for K and P () From matrix inversion lemma = P P = ( I n K )P 1 P H 1 R P his covariance update does not inherently preserve symmetry With P () nown, estimation gain is K = ( I n K )P H R 1 = P ( )H 1 R 1
Second-Order Example of Kalman Filter Rolling motion of an airplane, continuous-time Δ p Δ φ = L p Δp 1 Δφ L δ A Δδ A L p Δp w Rolling motion of an airplane, discrete-time Δp Δφ = Roll rate, rad/s Roll angle, rad Δδ A = Aileron deflection, rad Δp w = urbulence disturbance, rad/s Δp Δφ = ( e Lp 1) 1 L p e L p ϕ 11 = ϕ 21 1 Δp 1 Δφ 1 Δp 1 Δφ 1 γ 1 ~ L δa ΔδA 1 ΔδA 1 λ 1 Δp w 1 ~ L p Δp w 1 = sampling interval, s 11 Second-Order Example of Kalman Filter Rate and Angle Measurement Δp M Δφ M = Δp Δn p Δφ Δn φ = IΔx Δn 1) State Estimate Extrapolation Δˆp Δ ˆφ = ϕ 11 ϕ 21 1 Δˆp 1 Δ ˆφ 1 γ 1 Δδ A 1 12
Second-Order Example of Kalman Filter 2) Covariance Extrapolation p 11 p 21 p 12 p 22 = ϕ 11 ϕ 21 1 p 12 ( ) p 22 ( ) p 11 p 21 1 ϕ 11 ϕ 21 1 σ p 2 Q 1 L p Q' L C p = 2 σ p 3) Gain Computation 11 12 21 22 = p 11 p 21 p 12 p 22 p 11 p 21 p 12 p 22 2 σ pm 2 σ φm 1 R δ j = 2 σ pm 2 σ φm 13 Second-Order Example of Kalman Filter 4) State Estimate Update Δˆp Δ ˆφ = Δˆp Δ ˆφ 11 12 21 22 Δp M Δφ M Δˆp Δ ˆφ 5) Covariance Update p 12 ( ) p 22 ( ) p 11 p 21 = p 11 p 21 p 12 p 22 1 σ pm 2 1 2 σ φm 1 14
Example: Propagating a Scalar Probability Density Function Scalar LI system with zero-mean Gaussian random input x 1 = bx 1 b 2 u 1 b 2 w, x given Propagation of the mean value x i1 = bx i 1 b 2 u i, x given Propagation of the variance P 1 = b 2 P ( 1 b 2 )Q, P given 2 Q = E( w ) 15 Example: System Response to Disturbance, Large Measurement Error Large initial uncertainty 16
Measurement Error and State Estimate Standard Deviations 17 Actual State, Estimated State and Standard Deviation! High measurement error! State estimate emphasizes system model 18
Example: System Response to Disturbance, Small Measurement Error Large initial uncertainty 19 Measurement Error and State Estimate Standard Deviations 2
Actual State, Estimated State and Standard Deviation! Small measurement error! State estimate emphasizes measurements 21 Linear-Optimal Predictor t : Current time, sec t K : Future time, sec State estimate propagation from last estimate ˆx K = Φ( t K t ) ˆx ( ) Γ ( t K t )u Covariance estimate propagation P P K = Φ t K t Q ( t K t ) Φ t K t Predictor analogous to Kalman filter without measurement 22
Prediction Depends Entirely on the Model Bad model bad prediction High disturbance covariance bad prediction Prediction of mean might seem good but if covariance grows, uncertainty may render prediction meaningless 23 Discrete-ime Linear-Optimal Prediction, 1 points 24
Discrete-ime Linear-Optimal Prediction, u =.2 sin /2π, 1 points 25 Alternative Forms of the Kalman Filter 26
Simplifying the Gain Calculation Filter gain computation for r measurements K = P P r x r inversion required H R 1 K = For r = 1 P P H r scalar division 27 Consider One Measurement at Each Sampling Interval With r measurements and diagonal R, consider just one measurement at a time z = z 1 z 2 z r r 11 r ; R = 22 ; H = r rr ˆx = Φ 1 ˆx 1 Γ 1 u 1 ˆx ( ) = ˆx K i z i H i ˆx Scalar Update K i = P H i H i P H i r i Cycle through all measurements varying H, and repeat cycle Wasteful, as it may not use all available information H 1 H 2 H r z 1 = z 1 z 2 = z 2 z r = z r 28
Sequential Processing of Measurements at Each Sampling Interval With r measurements, form an inner loop of calculations, processing one sample at a time for i = 1,r K i = P i1 ( )H i H i P i1 ( )H i r i P i ( ) = ( I n K i H i )P i1 ( ), P end ˆx i ( ) = ˆx i1 ( ) K i z i H i ˆx i1 ( ) = ˆx r ( ) = P r ( ) ˆx P = P z 1 = z 1 z 2 = z 2 z r = z r H 1 = H 1 H 2 = H 2 H r = H r 29 ε Joseph Form of the Filter ( Stabilized Kalman Filter ) Guaranteed to retain positivedefiniteness and symmetry State update is ˆx ( ) = [ I n K ] ˆx K z Pre- and post-update measurement errors = x ˆx ; ε [ ]ε ε ( ) = I n K = x ˆx ( ) Measurement error is updated by K n 3
Joseph Form of the Filter ( Stabilized Kalman Filter ) ε E ε ε [ ]ε = I n K Definitions K n = P ; E( n n ) = R ; E( ε n ) = hen, covariance update is the outer product of the expected error P ( ) = [ I n K ]P [ I n K ] K R K Update equation is symmetric Equation updates covariance whether or not K is optimal Evaluate error covariance of a sub-optimal filter Design and evaluate a low-order filter for a high-order system Does not require an (n x n) inversion 31 P : Information Matrix Form of the Kalman Filter Filter based on the inverse of P State error covariance (small is good) I = P 1 : Information matrix (large is good) Fewer continuing inversions = P 1 H R 1 P I ( ) = I H R 1 1 32
Information Matrix Propagation and Gain Matrix I 1 = I n B 1 Λ 1 A 1, I = P where A 1 = Φ 1 1 I 1 ( )Φ 1 B 1 = A 1 Λ 1 1 1 A 1 Λ 1 Q' 1 and K = I 1 ( )H 1 R 33 Ill Conditioning of the Filter Computation Calculations may be inaccurate if system contains very fast and slow modes very noisy and near-perfect measurements very large and small disturbance inputs Condition number of a matrix, P, is the ratio of singular values = λ max P P P λ min ( P P) 1/2 = σ max P P σ min = σ ( P) σ ( P) 34
Solutions to Ill Conditioning Double, triple,..., precision arithmetic, or Formulate the equations to solve for the square root of P Define P! SS or S S then ( P) = ( SS ) = 2 ( S), which is order 1 x ( S) = ( P), which is order 1 x/2 35 U-D Formulation of the Kalman Filter U : Factorization of P P = UDU ( UD 1/2 )( UD 1/2 ) SS where Unit upper triangular matrix: D : Diagonal matrix: " " " 1 " " 1 " 1 No square roots in the factorization, but formulation has squareroot conditioning Covariance update uses sequential processing Algorithm originally expressed in pseudo-code (Bierman and hornton, 1977) See OCE (pp. 357-36) for equations and pseudo-code 36
Kalman Filters for More Complex Systems* Correlated Disturbance Input and Measurement Error Measurements may be corrupted by the same processes that force the system (e.g., turbulence) Consider estimation-error dynamics, as in Joseph form Stepwise minimization of estimation cost function w.r.t. filter gain matrix See OCE ime-correlated ( Colored ) Measurement Error Augment system with measurement error dynamics Use measurement differencing wo-step state and covariance estimates See OCE * Arthur Bryson and students 37 Next ime: Kalman-Bucy Filters for Continuous-ime Systems 38
Supplemental Material 39 E x Descriptions of Random Variables = ˆx o ; E ( x ˆx o )( x ˆx o ) E u E( w ) = ; E( w j w ) = Q' δ j [ ] = u ; E [ u u ][ u u ] = P { } = E( n ) = ; E n j n = E w j n = R δ j 4
Program for Covariance Propagation and Kalman Filter Estimatation % Scalar Propagation of Mean and Variance % Kalman Filter % Copyright by Robert Stengel. All rights reserved. % 4/12/211 clear '==========================' 'Propagation of Uncertainty' date b =.99 b2 = b*b bb = 1 - b^2 sqrb = sqrt(bb) w = []; x = []; xbar = []; x(1) = 1 xbar(1) = x(1) P(1) = 3 Q =.1 u = for = 1:999 w() = sqrt(q)*randn(1); x(1) = b*x() sqrb*u sqrb*w(); xbar(1) = b*xbar() sqrb*u; P(1) = b2*p() bb*q; end = [1:1]; w(1) = sqrt(q)*randn(1); figure plot(,w,'c',,x,'b',,(xbar sqrt(p)),'',,(xbar - sqrt(p)),'',,xbar,'r'),grid title(['propagation of Uncertainty, b =:', num2str(b), ', Q =:',num2str(q)]), xlabel('sampling Index, '), ylabel('x, xbar, xbar ± sqrt(p)') legend('disturbance','state','mean Sigma','Mean - Sigma','Mean') % Kalman Filter '=============' 'Kalman Filter' xm(1) = xp(1) = Pm(1) = 1 Pp(1) = 1 z(1) = xm(1) Q = Q R =.1 u = for = 1:999 n() = sqrt(r)*randn(1); xm(1) = b*xp() sqrb*u; Pm(1) = b2*pp() Q; K = Pm(1) / (Pm(1) R); z(1) = x(1) n(); xp(1) = xm(1) K*(z(1) - xm(1)); Pp(1) = 1 / ((1/Pm(1)) 1/R); end = [1:1]; n(1) = sqrt(r)*randn(1); figure plot(,z,'c',,(xp sqrt(pp)),'g',,(xp - sqrt(pp)),'g',,xp,'r',,x,'b'),grid title(['kalman Filter, Q =:',num2str(q),', R =:',num2str(r)]), xlabel('sampling Index, '), ylabel('x, xp, xp ± sqrt(pp)') legend('measurement','estimate Sigma','Estimate - Sigma','Estimate','Actual') % Covariance Comparison figure SP = sqrt(p); SPp = sqrt(pp); plot(,n,'c',,w,'g',,sp,'b',,spp,'r'), grid title(['covariance Comparison, Q =:',num2str(q),', R =:' num2str(r)]), xlabel('sampling Index, '),ylabel('w, n, Sqrt(P), Sqrt(Pp)') legend('measurement Error','Disturbance','Estimate of Actual State Std Dev','Estimate Error Std Dev') 41