Adaptive State Estimation Robert Stengel Optimal Control and Estimation MAE 546 Princeton University, 218! Nonlinearity of adaptation! Parameter-adaptive filtering! Test for whiteness of the residual! Bias estimation! Noise-adaptive filtering! Multiple model estimation 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 Nonlinearity of Adaptation Adaptation required if System parameters are unknown System structure is unknown Disturbance/measurement statistics are uncertain Adaptive estimators are fundamentally nonlinear, even if the system is linear Parameters to be estimated multiply the state Statistics are derived from measurement residuals Estimator gain depends on parameter estimates 2
Parameter Variation Approximated as Process Noise White noise disturbance input ( process noise ) is similar to random parameter variation w'!x = F( p)x + Gu + Lw " F( p o )x t + F ( p o ) p Δp t # F( p o )x + Gu + L w L Δp x p w t Δp t ; E w' x + Gu + Lw F p o Δp is an ( n n) matrix L Δp x t is n s = ; E w' w' T τ w t Δp t matrix { } = Q'δ ( t τ ) however, model is approximate and nonlinear 3 Parameter-Adaptive Estimation (Parameter Identification via Extended Kalman Filter) 4
Parameter-Dependent Linear System Linear system with parameterdependent sensitivity matrices x = F p x t + G( p)u + L p = Hx + n z t w t E w t n t = ; E w t n t w T ( τ ) n T ( τ ) = Q R δ t τ Parameter vector, p(t), could be Known: a prescribed function of time, and system is LTV Unknown: the output of a random dynamic process 5 Dynamic Model for Parameter Estimation Augment vector to include original state and parameter vector x A System is nonlinear: parameter is in the augmented state x A = z t F p t x t = H A p + G p f p x t p t x t p t u + L p w x f A x,p,u,w p,w p + n = H p x { } p + n + b p H p t : Unknown scale factors and coupling terms (TBD) b p t : Unknown bias error (TBD) BUT What is the model for time variation, p(t)? 6
Parameter Vector Must Have a Dynamic Model Unknown constant: p(t) = constant p = ; p( ) = p o ; P p ( ) = P po Random p(t) (integrated white noise) p = w p ; p( ) = ; P p ( ) = P po E w p = ; E w p w T p ( τ ) = Q pδ t τ Linear dynamic system (Markov process) p = Ap + Bw p f p p t,w p t ; w p t N,Q p 7 Dynamic Models for the Parameter Vector Doubly integrated white noise p M = p t p D = I p t p D + w p t p = p D Triply integrated white noise Parameter vector Parameter rate of change p M = p t p D p A = I I p t p D p A + p p D p A w p = Parameter vector Parameter rate of change Parameter acceleration 8
Integrated White Noise Models of a Parameter Third integral models slowly varying, smooth parameter Second integral is smoother but still has fast changes First integral of white noise has abrupt jumps, valleys, and peaks White noise 9 Hybrid Filter for Parameter Estimation P A t k ( ) ˆx A t k ( ) = x t A ( + ) k 1 + f A ˆx A ( τ ),u( τ ) dτ = P t A ( + ) k 1 + F A ( τ )P A τ = P A t k ( ) K t k Extrapolation of Augmented State t k t k 1 t k t k 1 Covariance Extrapolation H T A ( t k ) H A t k + P A τ F A T τ Filter Gain Calculation P A t k + L A τ H T A Q' C ( τ )L T A ( τ ) dτ ( t k ) + R t k 1 ( F A,H A ) must be locally observable except (perhaps) at isolated points 1
Hybrid Filter for Parameter Estimation ˆx A t k ( + ) = ˆx A t k P A t k + State Update { } + K ( t k ) z( t k ) h ˆx t k ( ) Covariance Update = I n K( t k )H A ( t k ) P A t k 11 Example: Estimation of Variable Rotary Load for a Robot Arm Elbow x 1 (t) x 2 (t) Parameter, p t Closed-loop dynamic equation = 1 c 1 / J t c 2 / J Rotary load inertia, J t x 1 (t) x 2 (t) + c 1 / J y c ; c 1,c 2 : Control gains (given) 12
x A = z = Augmented State and Measurement for Unknown Inertia Modeled as Doubly Integrated Parameter x 1 x 2 J J 1 1 x 1 x 2 p t p D x 1 x 2 p t Angle Angular Rate Rotary Load Inertia p D Inertia Rate + n 1 n 2 13 Nonlinear Dynamic Equation with Unknown Inertia Modeled as Doubly Integrated White Noise x A = x 1 x 2 p t p D + = c 1 / p t 1 1 c 1 / p t c 2 / p 1 y c + w 2 w pd t f A [ ] x 1 x 2 p t p D 14
Stability and Measurement Matrices F A = 1 1 c 1 2 ˆp 2 c 1 + 2 2 ˆp 2 2 1 c 1 / ˆp c 2 / ˆp H A = 1 1 15 P A Means and Covariances ˆx A ( ) = E x 1 ( ) = E x A ( ) ˆx ( A ) ˆx 1 ( ) p( ) ˆp ( ) T { x A ( ) ˆx A ( ) } ŵ = ; Q = E w t ˆn = ; R = E n t w T ( τ ) n T ( τ ) 16
Hybrid Filter for Robot Elbow State and Load Estimation Extrapolation of Augmented State Covariance Extrapolation Convergence is problem-dependent Qualitative observability of parameter Actual and assumed uncertainty covariances Accuracy of dynamic model P A t k ( ) ˆx A t k ( ) = x t A ( + ) k 1 + f A ˆx A ( τ ),u( τ ) dτ = P A t k 1 ( + ) + F A ( τ )P A τ t k t k 1 + P A τ t k t k 1 F A T τ + L A τ Q' C ( τ )L T A ( τ ) dτ Filter Gain Calculation State Update Covariance Update = P A t k ( ) K t k ˆx A t k ( + ) H T A ( t k ) H A t k = ˆx A t k + K t k P A t k ( ) H T A ( t k ) + R t k z( t k ) h ˆx t k ( ) 1 { } P A t k ( + ) = I K t n ( k )H A ( t k ) P A t k ( ) 17 Weathervane Example (OCE, 4.7-1) True System!x 1!x 2 = 1 4.8 z 1 z 2 = 1 1 x 1 x 2 Q = q = 1; R = x 1 x 2 + w 4 + n 1 n 2 1 1 Assume F(2,1) is an unknown constant, a Linear Dynamic System for Covariance Estimation Δˆ! x1 Δˆ! x2 Δ ˆ! a = 1 â.8 ˆx 1 Δˆx 1 Δˆx 2 Δâ + â Δw 18
Weathervane Example (OCE, 4.7-1) State/Parameter Estimator ˆ!x 1 ˆ!x 2 ˆ!a ˆx 2 = âˆx 1.8 ˆx 2 ˆx 1 ˆx 2 â Initial Conditions for State/ Parameter Estimation = 4.4 k 11 k 12 + k 21 k 22 k 31 k 32 ; P = z 1 ˆx 1 z 1 ˆx 2 2! Parameter estimate approaches constant value as t, p 13, p 23, p 33 t p 33 â ˆx 1! Estimate may be biased 19 Bias Estimation and Noise-Adaptive Filtering 2
Residuals and Optimal Filtering Linear-optimal filtering has the innovations property r k a) Optimal estimation extracts all the available information from the measurements b) Optimal measurement residual should be zeromean white noise c) State estimate should be orthogonal to the error Residual and its statistics ( )! z k Hˆx k ( ) = H x k ( ) ˆx k ( ) + n k E r k! ˆr k E r k ( )r T k ( )! S k T E ˆx k ( )n k = E Φˆx k 1 + = = HP k H T + R k State estimate is orthogonal to the measurement noise T { + Γu k 1 n k } = 21 Residual Should Be White Noise if State Estimate is Optimal C( k) = Test for whiteness using normalized autocovariance function Sampled (batch process) estimate of the autocovariance function matrix, C(k) N 1 N 1 r n ( )r T n+k ( ), k (integer) << N n=k k dim C k = r r Normalize diagonal elements of C(k) by their zero-lag (k = ) values ρ ij ( k) = c ij k c ii 22
Test for Residual Whiteness If r k is white ρ ij ( k) = 1, i = j and k =, i j or k, N Off-diagonal terms should be negligible For finite sample, 95% confidence limit based on diagonal elements ρ ii ( k) 1.96 N, k Test is passed if 19 out of 2 non-zero-lag samples are within the limit Mehra, IEEE TAC, 197 23 Bias Example: Advanced Dependent Surveillance (ADS-B) System for Air Traffic Control! Surveillance and tracking radars on ground! GPS/Inertial/Air data measurements in aircraft! Satellite/Line-of-sight communications links! Air traffic control centers (ATCC)! Prevent collisions! Maintain efficient flow of air traffic 24
Dynamics of Individual Aircraft Simplified surveillance equations of motion* Neglect fast (attitude) dynamics of aircraft x y z v x v y v z = 1 1 1 x y z v x v y v z + w x w y w z * Latitude, longitude, and altitude rather than (x, y, z) 25 Air Traffic Control Center Measurements and Communicated Locations of Individual Aircraft Ground-based radar measurements GPS measurements z = Aircraft Inertial navigation measurements Air data measurements = ( Hx + n) + b b = Biases = Radar Range Bias Radar Azimuth Bias Aircraft Altimeter Bias ADS-B Longitude Bias ADS-B Latitude Bias ADS-B Altitude Bias ADS-B North Velocity Bias ADS-B East Velocity Bias Bias is a quasi-constant error that is not related to random noise Least-squares estimator does not reduce bias error 26
Measurement Bias and Covariance Estimation Batch processing estimates r ( ) ˆb ( ) = 1 N N i=1 r i ( ) = 1 N N i=1 z i Hˆx i ( ) Bias Estimate ˆR = 1 N ( r i r i )( r i r i ) T N 1 N 1 N HP i ( )H T i=1 = Ŝ HP av ( )H T, for large N Measurement Noise Estimate where Ŝ = 1 N 1 N i=1 ( r i r i ) r i r i T Sample Covariance Matrix 27 Running Estimate of Measurement Bias and Error Covariance { ˆb k 1 } ˆb k = ˆb k 1 + k bias z k Hˆx k ˆR k = ˆR k 1 + k noise = ˆb k 1 + k bias r k ˆb k 1 = [ 1 k bias ] ˆb k 1 + k bias r k { T HP ( k )H } T ( r k ˆb k ) r k ˆb k k bias, k noise < 1 ˆR k 1 Options! Choose ad hoc recursive (digital filter) gain! Use weighted least-squares estimator! Incorporate in an integrated parameter-adaptive filter 28
Disturbance Bias Estimation System equation x k +1 = Φx k + w k Disturbance residual w k = Φx k x k +1 Φˆx k + ˆx k +1 ( + ) w ŵ ( + ) = 1 N Sample mean N i=1 Φˆx i ( + ) ˆx i+1 ( + ) = 1 N N i=1 w i 29 Disturbance Covariance Estimation Ŵ = Sample covariance 1 N 1 N i=1 ( w i w) w i w T Disturbance covariance estimate ˆQ = Ŵ N 1 N ΦP av ( + )Φ T! Ŵ ΦP av ( + )Φ T 3
Running Estimate of Disturbance Bias and Covariance { } ŵ k ( + ) = ŵ k 1 ( + ) + k bias w k ( + ) ŵ k 1 ( + ) ˆQ k = ˆQ k 1 + k noise w k ŵ k ( w k ŵ k ) T N 1 k bias, k noise < 1 N ΦP k ( + )Φ T ˆQ k 1! Options as before! Choose add hoc recursive gain! Use weighted least-squares estimator! Incorporate in an integrated parameter-adaptive filter 31 Noise-and-Bias Adaptive Filter Use means and covariances from ad hoc estimation in Kalman filter ˆx k ( ) = Φ k 1 ˆx k 1 ( + ) + Γ k 1 u k 1 + ŵ k 1 ( + ) P k K k = P k T ( ) = Φ k 1 P k 1 ( + )Φ k 1 + ˆQ k 1 T ( )H k H k P k ˆx k ( + ) = ˆx k ( ) + K k z k H k ˆx k ( )H T k + ˆR k 1 + ˆb k = P 1 T k ( ) + H k ˆRk 1 H k P k + 1 or incorporate as parameters in a unified parameteradaptive filter 32
Multiple Model Estimation 33 Multiple Model Estimation! Bank of Kalman filters, each tuned to a different hypothesis! Different model parameters or structures! Different uncertainty models! Different initial conditions! Best performance determined by a hypothesis test, e.g., Maximum Likelihood! State estimate chosen accordingly 34
Hypothesis Testing 35 Multiple Model Estimation 36
Multiple Model Estimation! Consider J systems distinguished by J parameter vectors! Conditional probability mass function for the j th parameter set Pr( p j z k ) = I i=1 pr( z k p j )Pr( p j ) k 1 pr( z k p i )Pr p i k 1! Probability that the measurement at k 1 was obtained is one; therefore, Pr( p j ) k 1 = Pr( p j z k 1 )!... and the equation forms the basis for a recursion Pr( p j z k ) = J i=1 pr z k p j pr( z k p i )Pr p i k 1 Pr( p j z k 1 ) 37 Multiple Model Estimation Conditional probability density function z k must be found With the true parameter set z k = Hx k + n k x k = Φx k 1 + Γu k 1 + w k 1, x o = x pr z k p If the true state were known = pr z k x k ( p) 1 = ( 2π ) n/2 1/2 R e k 1 = ( 2π ) n/2 1/2 R e k 1 ( 2 z k Hx k ) T R 1 k ( z k Hx k ) 1 2 n k T R k 1 n k However, only an estimate of x k is available 38
Multiple Model Estimation! The state is estimated by a Kalman filter for the true parameter pr z k ˆx k ( p) = 1 2π n/2 S k 1/2 e 1 2 r k T S k 1 r k with r k ( ) = z k Hˆx k ( ) S k = HP k ( )H T + R k! The bank of J Kalman filters is formed with each filter assuming that different parameters are the true parameter 39 Conditional Probabilities and the Adaptive State Estimate Conditional probabilities for each hypothesis Pr( p j z k ) = J i=1 ( p j ) Pr ( p j z k 1 ) { pr z k ˆx k ( p i ) Pr( p i z k 1 )} pr z k ˆx k, j = 1, J A) State estimate: Estimate with highest conditional probability ˆx k ( + ) = ˆx k p k * [ ], p k * = argmax Pr( p i z k ), i = 1, J B) State estimate: Probability-weighted sum of the state estimates = Pr( p i z k ) ˆx k p i,( + ) ˆx k + J { } i=1 4
Weathervane Example (OCE, 4.7-2)! 2 nd -order system with three hypothesized natural frequencies ω 2 n = [ ] [ ] 4.8 4 Correct 4.4 Expected ζ =.1! Option B: Algorithm searches, then homes in on correct solution 41 Caveats Each hypothesis requires a separate filter Number of required parallel filters may be large if all combinations are possible N # of filters = n i # of values, i = 1, N # of parameters i=1 True parameters may lie between tabulated values Solutions Pruning Sequentially eliminate least likely combinations Adaptive hypothesis adjustment ( parameter tracking ) Analogous to particle filtering; track most likely combinations, adjust distribution, drop low-probability hypotheses 42
Estimating Parameters of Nonlinear Systems Using Optimal Smoothing 43 Estimation Before Modeling* 1. Incorporate the unknown parameters in the state 2. Estimate the state using optimal smoother 1. Extended Kalman Filter (forward pass) 2. Modified Bryson-Frazier Smoother (backward pass) 3. Use multivariate regression to model aerodynamic coefficients as functions of state and control Measured vs. Estimated Yaw Rate 44
Smoothing vs. Filtering Filtering introduces a lag in the estimate The estimator is a Low-Pass Filter Causal, i.e., it uses measurements up to the current time but has no information about the future state Smoothing introduces no lag in the estimate The estimator is a Low-Pass Filter Non-Causal, i.e., it uses measurements before and after the current time Can Implement as forward and backward processing with lowpass filter H smoother H filter ( s) = ( s) = a s a, causal a 2 ( s a) s + a, non-causal * See Bryson & Frazier, 1963; Rauch, Tung, Streibel, 196;, Fraser, 1967; Bryson & Ho, 1969; Bierman, 1973, 1977; and papers on square-root implementations 45 Bryson-Frazier Optimal Smoother Smoothing viewed as deterministic optimal control problem * min J est = 1 x( t ), w, n 2 ΔxT (t )P 1 (t )Δx(t )+ 1 2 tf t w T (t ) n T (t ) Q 1 R 1 w(t ) n(t ) dt Subject to constraints Δx(t ) = x(t ) E [ x(t )]!x ( t ) = f x( t ), t + g x( t ), t w t = h x t z t, t + n(t ) Introduce adjoint vector, Hamiltonian, * Bryson, Frazier, 1963; Bryson, Ho, 1969 46
Modified Bryson-Frazier Smoother * Motivated by Rauch-Tung-Streibel but viewed as recursive form of Bryson-Frazier Augment Hybrid Extended Kalman Filter results with backward pass through data from final to initial time Initialize adjoint variable and its covariance at final time, t f : λ f Λ f ( ) = H T f D 1 f ( )Δz f ( ) = H T f D 1 f ( )H f where T D f ( ) = H f P f ( )H f Δz f ( ) = z f h ˆx f ( ) * Bierman, 1973 47 Modified Bryson-Frazier Smoother Adjoint vector and covariance differential equations λ k Λ k!λ t = F T t = F T t!λ t λ ( t ) Λ t Λ T ( t ) F( t ) Propagate adjoint vector and covariance from final to initial condition λ k ( + ) = λ k+1 Λ k ( + ) = Λ k+1 Update adjoint vector and covariance ( ) = λ k + ( ) = I K k H k t k ( + ) F T t t k+1 ( ) t k ( + ) F T t t k+1 ( ) λ ( t )dt Λ t Λ T ( t ) F( t ) H k T D k 1 Δz k + D k K k T T Λ k+1 ( + ) I K k H k dt + H k T D k 1 H k 48
Modified Bryson-Frazier Smoother Smoothed estimates of state and covariance are corrections to filter estimates ˆx k + = ˆx k ( + ) P k ( + )λ k ( + ) ˆ P k ( + ) = P k ( + ) P k ( + )Λ k ( + )P k ( + ) 49 Schweizer 2-32 Aerodynamic Coefficients of a Sailplane from Flight Data* Princeton University Flight Research Laboratory * Sri-Jayantha, M., and Stengel, R., Determination of Aerodynamic Coefficients Using the Estimation-Before-Modeling Method, J. Aircraft, 25(9), 1988 [on Blackboard] 5
Dynamic Equations of the Sailplane (Attitude from 4-Element Quaternion Vector, q) Position!r I = H B I v B!q = Qq = fcn ω B Rotation Matrix and Inverse H B I, H I B are functions of q Velocity Quaternion!v B = 1 m F B + H I B g I "ω B v B Angular Rate!ω B = I B 1 M B "ω B I B ω B See MAE 331 Lecture 9 slides, http://www.princeton.edu/~stengel/mae331lectures.html 51 F B = I xx I xy I xz I B = I xy I yy I yz I xz I yz I zz X aero Y aero Z aero Inertia Matrix = B C X C Y C Z B Aircraft Characteristics Expressed in Body Frame of Reference 1 2 ρv 2 S B b = wing span c = mean aerodynamic chord S = wing area Aerodynamic force and moment vectors M B = L aero M aero N aero C l b = C m c C n b B B 1 2 ρv 2 S Force and moment coefficients modeled as triply integrated white noise 38-component state vector 52
Normal Force and Lift Coefficient Estimates Smoothed Estimate of Normal-Force Coefficient (C z ) History, including Dynamic Effects Binned Estimate of Lift Coefficient (C L ) Static Dependence on Angle of Attack Regression applied within angle-of-attack bins 53 Next Time: Stochastic Optimal Control 54