Nonlinear State Estimation! Particle, Sigma-Points Filters! Robert Stengel! Optimal Control and Estimation, MAE 546! Princeton University, 2017!! Particle filter!! Sigma-Points Unscented Kalman ) filter!! Transformation of uncertainty!! Propagation of mean and variance!! Helicopter, HIV state estimation eamples!! Additional nonlinear filters Copyright 2017 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 Criticisms of the Basic Etended Kalman Filter* *Julier and Uhlmann, 1997; Wan and van der Merwe, 2001! State estimate prediction is deterministic, i.e., not based on an epectation! Not true; epectation is the same as the deterministic prediction)! State estimate update is linear! Jacobians must be evaluated to calculate covariance prediction and update! Not all comments apply to iterated, quasilinear, or adaptive etended Kalman filters 2
Transformation of Uncertainty Nonlinear transformation of a random variable :Random variable with mean,, and covariance, P [ ] y = f Estimate the mean and covariance of the transformation s output y,p ) and P yy,p ) The transformation is said to be unscented * if its probability distribution is Consistent, Efficient, and Unbiased *! Julier and Uhlmann, 1997 3 Consistent Estimate of a Dynamic State Let k!, k+1! y k+1 k,p k k ) = y,p ) and P k+1 k+1 k,p k k ) = P yy,p Consistent state estimate converges in the limit { P k+1 k+1! E " k +1! k +1 ) k +1! k +1 ) T } 0 { Estimated Covariance Actual Covariance} 0 Lesson: In filtering, add sufficient process noise to the filter gain computation to prevent filter divergence ) 4
Adding Process Noise Improves Consistency Filter s estimate of RMS state error Actual state estimate error { P k+1 k+1! E " k+1! k+1 ) k+1! k+1 ) T } 0 { Estimated Covariance Actual Covariance} 0!! Satellite orbit determination! Aerodynamic drag produced unmodeled bias! Optimal filter did not estimate bias Process noise increased for filter design! Divergence is contained Filter s estimate of RMS state error Actual state estimate error Fitzgerald, 1971 5 Efficient and Unbiased Estimate of a Dynamic State Efficient state estimator converges more quickly than an inefficient estimator min { Estimated Covariance Actual Covariance} Added Process Noise Add just enough process noise Unbiased estimate k +1 = E k +1 ) [Estimated Mean = Actual Mean] 6
Empirical Determination of a Probability Distribution! Monte Carlo evaluation! Propagate random noise through nonlinearity for given prior distribution e.g., Gaussian)! Generate histogram of output! Repeated evaluation N trials) of nonlinear system propagation! Use histogram as numerical representation of distribution, or! Identify associated theoretical distribution using functional minimization!! Particle Filter! Propagate mean, E), using the empirical probability distribution! As N > ", estimate error > 0 7 Empirical Determination of Mean and Variance! Sample mean for N data points, 1, 2,..., N = N! i=1 N i! Sample variance for same data set! 2 = i " ) 2! Divisor is N 1) rather than N to produce an unbiased estimate N i=1 N " 1)!! Sigma Points Filter:!! Propagate mean, E), using the sample mean and variance 8
Sigma Points of P and Mean 2-D Eample 9 Sigma Points of P and Mean 3-D Eample 10
Sigma Points of Estimate Uncertainty, P State covariance matri P : Symmetric, positive-definite covariance matri Eigenvalues are real and positive si n! P = s! " 1 ) s! " 2 )! s! " n Eigenvectors )! i I n " P )e i = 0, i = 1,n Modal matri E =! e 1 e 2! e " n 11 Sigma Points of P Diagonalized covariance matri Eigenvalues are the Variances 1 0! 0 ) 0! = E "1 P E = E T P E = 2! 0 )!!!! ) = ) 0 0! n ) * 1 2 0 * 2 2 0! 0! 0!!!! 0 0! 2 * n ) ) ) ) ) 1) Principal aes of the covariance matri are defined by modal matri, E 2) Location of 2n one-sigma points in state space given by ±! " 1 ) ±! " 2 )! ±! " n ) = E ±" 1 0! 0 0 ±" 2! 0!!!! 0 0! ±" n 12
Propagation of the Mean Value and Covariance Matri! 13 Propagation of the Mean Value and the Sigma Points!! Mean value at t k t k ) = k!! Sigma points relative to mean value) k " k! i ), i = 1,n! ik! k + k! i ), i = n +1),2n!! Projection from the prior mean k +1 = k + t k+1 t k f!" t),u t),w t),t dt Nonlinear projection from each prior sigma point t k+1! ik+1 =! ik + f "! i t),u t),w t),t dt, i = 1,2n t k 14
Estimation of the Propagated Mean Value! Assumptions:!! To 2 nd order, the propagated probability distribution is symmetric about its mean New mean is estimated as average or weighted average of projected points arbitrary choice by user) Ensemble Average for the Mean Value ˆ k +1 = k +1 + 2n " i=1 2n + 1! ik+1 Weighted Ensemble Average for the Mean Value 2n " ik+1 k+1 +! i=1 ˆ k+1 = 2!n +1 15 Projected Covariance Matri Unbiased ensemble estimate of the covariance matri 1 2n +1 )!1 P k+1 k+1 = k+1! ˆ k+1 ) k+1! ˆ k+1 2n ) T + " ik+1! ˆ k+1 i=1 )" ik+1! ˆ k+1 ) T This estimate neglects effects of disturbance uncertainty during the state propagation from t k to t k+1 Does not require calculation of Jacobian matrices ) 16
Sigma Points of Disturbance Uncertainty, Q Q : s! s) Symmetric, positive-definite covariance matri si s " Q = s " 1 ) s " 2 )! s " s i I s " Q)e i = 0, i = 1,s ) [s = Laplace operator] E Q = e 1 e 2! e s " 1 0! 0 0 "! Q = E T Q QE Q = 2! 0!!!! 0 0! " s Q = ) 1 2 Q 0 ) 2 2!! Eigenvalues!! Modal Matri 0!!! 0 Transformation! 0!!!! 0 0! 2 ) s Q ±!w " 1 ) ±!w " 2 )! ±!w " s ) = E Q ±" 1 0! 0 0 ±" 2! 0!!!! 0 0! ±" s Q 17 Propagation of the Disturbed Mean Value Sigma points of disturbance relative to mean value)! ik! w k + "w k i ), i = 1,s w k "w k i ), i = s +1),2s Incorporation of effects of disturbance uncertainty on state propagation t k+1!i ) = k+1 k + f t t k " ),u t),! i t),t dt, i = 1,2s 18
Estimation of the Propagated Mean Value with Disturbance Uncertainty Estimate now includes effect of disturbance uncertainty Estimate of the mean is the average or weighted average of projected points Ensemble Average for the Mean Value ˆ k +1 = 2n k +1 + "! ik+1 + " i i=1 2 n + s 2s i=1 ) + 1 ) k +1 Weighted Ensemble Average for the Mean Value ˆ k+1 = ) k+1 2n 2s k+1 +! " ik+1 + i i=1 i=1 2! n + s ) +1 * ) 19 Covariance Propagation with Disturbance Uncertainty Unbiased sampled estimate of the covariance matri 1 P k+1 k+1 =!" 2 n + s 1 P + P + P mean sigma disturbance ) +1 ) P mean = k+1! ˆ k+1 ) k+1! ˆ k+1 ) T 2n )" ik+1! ˆ k+1 ) T P sigma = " ik+1! ˆ k+1 i=1 2s P disturbance = i i=1 ) k+1! ˆ k+1 i )! ˆ k+1 k+1 T 20
Sigma-Points Unscented Kalman ) Filter! 21 System Vector Notation System vector! v! " dim v w n ) = n + r + s) 1 Epected value of system vector! ˆv 0 = " ˆ 0 ŵ 0 ˆn 0! = E " 0 w 0 n 0!! 0 = " 0 0 w 0 n Propagation of the mean! k+1! = h "," n t k+1 =! k + f "! t),u t),! w t),t dt ) t k Measurement vector, corrupted by noise Analogous to z t) = H t) + n t) 22
Matri Array of System and Sigma-Point Vectors Epected value of system vector! i = " ˆ 0! 0 = ˆv 0 = ŵ 0 ˆn 0 ˆv i + " S ˆv i " S ; dim! 0 ) i, i = 1, L ) i, i = L +1,2L S : Square root of P; S ) = n + r + s) 1! L 1 Weighted sigma points for system vector ); dim! i * ) = 2L +1 ) i! i th column of S Matri of mean and sigma-point vectors!! " 0 " 1 " " 2 L ; dim! ) = L 2L +1) 23 Initialize Filter State and covariance estimates P 0 ˆ o = E!" 0) = 0) " " 0)! ˆ 0 ) T { } ) = E 0)! ˆ 0 ) Covariance matri of system vector P v 0) = E " v 0)! ˆv 0 ) " v 0)! ˆv 0 ) T { } " = ) 0 0 ) 0 ) P 0 0 Q w 0 0 0 R n 0 24
Propagate State Mean and Covariance Incorporate disturbance sigma points! i ) k+1 =! i ) k + f "! i ) t ˆ k+1! 2 L ) = " i i i=0 t k+1 t k k+1 ) t w ),u t),! i ),t dt Ensemble average estimates of mean and covariance P k+1 2 L "! i =! i : Weighting factor Typically 1 L +1), i = 0 1 2 L +1), i = 1,2L { }!) = " i i!)! ˆ! ) k+1 i!)! ˆ! ) T k+1 i=0 after Wan and van der Merwe, 2001 25 Incorporate Measurement Error in Output Mean/sigma-point projections of measurement ) k +1 = h " i! i n ) k +1," i ) k +1, i = 0,2L Weighted estimate of measurement projection ŷ k+1! 2 L ) = " i i k+1 i=0 26
Incorporate Measurement Error in Covariance Prior estimate of measurement covariance y P k+1 2 L { }!) = " i i!)! ŷ! ) k+1 i!)! ŷ! ) T k+1 i=0 Prior estimate of state/measurement cross-covariance y P k+1 2 L ) { }!) = " i i!)! ˆ!) k+1 i!)! ŷ!) T k+1 i=0 after Wan and van der Merwe, 2001 27 Compute Kalman Filter Gain Original formula eq. 3, Lecture 18) T K k = P k!)h k H k P k!)h T!1 " k + R k Sigma points version w/inde change y K k +1 = P k +1 y!1!)" P k +1!) 28
Post-Measurement State and Covariance Estimate ˆ k +1 + ) = ˆ k +1!) + K k +1 z k +1! ŷ k +1!) P k +1 State estimate update " Covariance estimate update y T + ) = P k +1!)! K k +1 P k +1!)K k +1 29 Eample: Simulated Helicopter UAV Flight van der Werwe and Wan, 2004 30
Comparison of Pitch, Roll, and Yaw Errors 31 Comparison of RMS Errors for EKF, UKF, and Gauss-Hermite Filter GHF) EKF UKF GHF CD4+ Cells HIV Virions Effector Cells Banks et al, A COMPARISON OF NONLINEAR FILTERING APPROACHES IN THE CONTEXT OF AN HIV MODEL, 2010 32
Comparison of Various Filters for Blind Tricyclist Problem! Psiaki, 2013)! Etended Kalman filter! Unscented Kalman filter! Particle filter! Batch least-squares filter! Backward-smoothing EKF 33 Observations!! Jacobians vs. no Jacobians!! Number of nonlinear propagation steps!! Gaussian vs. approimate non-gaussian distributions!! Best choice of averaging weights is problemdependent!! Comparison of filters is problem-dependent!! Are these filters better than a quasi-linear filter? TBD) 34
More on Nonlinear Estimators!! Gordon, N., Salmond,D., and Smith, A. Novel Approach to Nonlinear/Non-Gaussian Bayesian State Estimation, IEE Proc.-F, 1402), Apr 1993, pp. 107-113.!! Banks, H., Hu, S., Kenz, Z., and Tran, H., A Comparison of Nonlinear Filtering Approaches in the Contet of an HIV Model, Math. Biosci. Eng., 7 2), Apr 2010, pp. 213-236.!! Psiaki, M., Backward-Smoothing Etended Kalman Filter, Journal of Guidance, Control, and Dynamics, 28 5), Sep 2005, pp. 885-894.!! Psiaki, M., The Blind Tricyclist Problem and a Comparison of Nonlinear Filters, IEEE Control Systems Magazine, June 2013, pp. 48-54.!! Psiaki, M., Schoenberg, J., and Miller, I., Gaussian Sum Reapproimation for Use in a Nonlinear Filter, Journal of Guidance, Control, and Dynamics, 38 2), Feb 2015, pp. 292-303. 35 Net Time:! Adaptive State Estimation! 36