无迹卡尔曼滤波（UKF）

【实例简介】
无迹卡尔曼滤波（UKF）原始文章，详述了UKF的原理和应用的例子
IEEE TRANSACTIONS ON AUTOMATIC CONTROL. VOL 45. NO. 3 MARCH Remark 4: The method can be generalized to include the effects of Comparison o: new filter and EKF process and observation noise by appending the noise vectors to the state vector [11] IV. EXAMPLE APPLICATION This section applies and compares the performance of the new filter d against an EKF for a tracking problem that was presented in [14]. this example was chosen because it has significant nonlinearities in the EKF process and observation models and has been analyzed extensively in the literature We wish to estimate the position 1(t), velocity 2(t), and constant 5 ballistic coefficient 3(t) of a body as it reenters the atmosphere at a very high altitude at a very high velocity. Its motion is determined by altitude- and velocity-dependent drag terms, and it is constrained to fall vertically. The position of the body is measured at discrete points in time using a radar capable of measuring range corrupted by Gaussian measurement noise. The radar is at an altitude of h(100 000 ft), and the horizontal range between the body and the radar, M, is(100o00 TIME (sec t The continuous time dynamics of this system are Fig 1. Absolute mean error position error (6) Comparison of new filler and EKF (t)2r(t)+"2(t) where au (t)are zero-mean, uncorrelated noises with covariances given by Q(t)and c is a constant(5 X 10 )that ates the air density with altitude. The range at time t, 2(t), is ≥2 z(t)=√(P+[a1(t)-m])+r(+) where r(l) is the uncorrelated observation noise with covariance 3 R(t)=10 ft. The measurements are made with a frequency of 1 Tracking systems were implemented using the new filter and the EKF. The nonlinearities of the process model and the high velocities required the numerical integration of (6)-(8)to be carried out using extremely small time steps. In accordance with [14], a fourth-order Runge- Kutta scheme was employed with 64 steps between each ob TIME(sec servation. For the ekf, it was necessary to recalculate the Jacobian 64 times between each update For the new filter, the trajectory of each sigma point was calculated wig the small time steps, but it was only Fig. 2. Absolute mean velocity error necessary to calculate the mean and covariance just before an observa tion was made. Because n=3, b was chosen to be zero in accordance runs. At high altitude, the drag effects are minimal, and the body falls with the heuristic n +k=3 approximately linearly. However, after about 10 s, drag becomes signif- The initial true state of the system is x(0)-[3x1052x10*10\, icant, motion becomes noticeably nonlinear, and the two filters differ nd the initial estimates and covariances of these states are significantly. The velocity estimates are shown in Fig. 2 and indicate largc crror spikes in both filters. Thesc occur when thc altitude of the (0|0)=[3×1052×1043×10-] body is the same as that of the radar and range information provides 10 0 less data about body notion. The new filter recovers quickly, but the P(O)=04×10°0 eKF has a larger error spike and only slowly converges. Fig 3 shows 010 the errors in estimating T3(). The error in the eKF estimate is biased and is an order of magnitude larger than that for the new filter Although the initial estimates of altitude and velocity are correct In Figs 4 and 5, we show the errors in the position estimates made X3(00) is very bad. The body is assumed to be"heavy, " whereas in by the EKF and the new filter and the associated estimates of the two reality it is"light. The behavior of the two filters differs if the second standard deviation bounds. These bounds are given by twice the square and higher order terms are significant. Because process noise can be root of the diagonals of the covariance matrix, and, if the filter is con used to mask linearization errors, we adopt the practice from [14] and sistent, the state errors should lie within these bounds 95%o of the time do not introduce any process noise into the simulation-Q(% )=0 However, the error in the ekf drifts outside of these bounds after 30 s for both filters showing that it does not yield consistent estimates. However, the errors In Fig. 1, we show the average magnitude of the state errors com- in the new filter always lie well within the two standard deviations, im mitted by each filter across a Monte Carlo simulation consisting of 50 plying that the new filter is consistent 480 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL 45, NO 3, MARCH 2000 Comparison of new filter and EKF NF mith 2 standard deviations FKF 10 000 NF 10 5101520255035404 TIME(sec) TIME (sec) Fig 3. Absolute mean error in 3 Fig. 5. New filter mean position error EKF with 2 standard deviations This algorithm has found a number of applications in high-order, nonlinear coupled systems, including navigation systems for high-speed road [21 ,[16 vehicles, public transportation systems 400 22], and underwater vehicles [23. Square root filters can be for mulated by discrete sets of points(as demonstrated in [24]), and iterated filters can be constructed using the predictions [25]. The algorithm has bccn xtended to capturc the first four moments of a Gaussian distribution 12 and the first three moments of an arbitrary distribution 201 Given its performance and implementation advantages, we conclude g-100 that the new filter should be preferred over the eKF in virtually all nonlinear estimation applications APPENDIX I HIGHER MOMENTS OF THE APPROXIMATION This appendix analyzes the properties of the higher moments of the sigma point selection scheme of (3). The selection process consists of TIME(sec) three stages. First, a set of sigma points are drawn to approximate an n-dimensional standard Gaussian with mean 0 and covariance. A Fig 4. EKF mean position error linear transformation [a matrix square root P(h k ) is applied to each point so that the transformed samplc still has mcan 0, but thc covari- ance is P(k k). Finally, x(k k)is added to each transformed sigma Therefore, we conclude that in this example the new filter has sub- point to ensure the correct sample mean h affects the first stage of the stantial advantages over the ekF both in implementation and perfor- approximation, and the matrix square root affects the second mance The sigma points, which are assumed approximate the standard Gaussian, lie on the coordinate axes. The orthogonality and the sym metry of these points means that the only nonzero sample moments V CONCLUSIONS are those that are an even order power of a single coefficient. the Motivated by the deficiencies of the EKF. we have examined a com- 2h th-order moment for any coefficient is(n + g)(k-l.Therefore pletely new approach for applying linear estimation theory to non- the higher order moments scale geometrically by a factor determined linear systems. Rather than approximate the Taylor series to an arbi by hi. Howcvcr, the moments for thc standard Gaussian arc diffcrcnt trary order, we approximate the first three moments of the prior dis- from those of the sigma points, which approximate it. Because the tribution accurately using a set of samples. The algorithm predicts the covariance matrix is I, the different components are independent of mean and covariance accurately up to the third order and, because the one another and higher order terms in the series are not truncated, it is possible to re duce the errors in the higher order terms as well. We have provided F[×…×]=∏F;] empirical evidence that supports the theoretical conclusion and have demonstrated that the new filter is far easier to implement because it does not involve any linearization steps, eliminating the derivation and 2In effect, these points are drawn from the orthogonal matrix square root of evaluation of jacobian matrices IEEE TRANSACTIONS ON AUTOMATIC CONTROL. VOL 45. NO. 3 MARCH 481 here the i th component in the moment is raised to the power ai.It a2 f 02f 771…11 +m112 (13) can be shown that [13] 0m-10 a: is odd E andnlc1c2.c: is thcith-order momcnt mc1c2:"Ci EL 1×3 is even The i th term is a function of the i th central moments of the distribu- Two diffcrcnccs exist in thc approximation by thc sigma points Lion ofx. Therefore, for an approximation to be accurate up to the ith First, the scaling"of nonzero moments are different. Second order, it must be able to approximate the moments correctly up to the number of moment terms are "missed out. These differences become ith order The ekf truncates this series after the first term and so its more marked as the order increases, but, as explained in Appendix Il. error in predicting the mean is in the second and higher orders. The new they only affect the higher order terms of the Taylor Series of the filter matches the mean and covariance correctly, and so it is correct up process model/nonlinear function. Assuming that the contribution of to the second order Because the new filter does not truncate the filter terms diminishes as the order increases, the emphasis is to minimize at any order, it can be expected that the errors in the fourth and higher the errors in the lowest order terms. Because e i 3 for the order terms are smaller than those committed by the EKF Gaussian distribution, choosing (n +ri)=3 minimizes the difference The covariance is given by Pyy ERy-y]y-y. Nov between the moments of the standard Gaussian and the sigma points p to the fourth order y-y=f+刘一[fx+刘] Because the fourth and higher order moments are not precisely D人 f daf da f D△xf+ + matched, the choice of matrix square root affects the errors in the 3 higher order terms by adjusting the way in which the errors are DAf d f distributed among the different states. However, in general, this infor- E\ 4! mation cannot be exploited because it would require knowledge of the higher order derivatives of the process model equation. In the absence with substitutions from(10)and(12 ). The true covariance is found of this information, the choice of matrix square root is governed by by postmultiplying the state error by the transpose of itself and taking other issues, such as numerical stability or computational cost expectations. Exploiting the fact that x is symmetric, the odd tcrms all evaluate to zero and the covariance is given by APPENDIXⅡI Pyy=VfPxr! MOMENT APPROXIMATION AND PERFORMANCE In this appendix we provide a justification for approximating a prob +E|D△f(D,f),D&f(Df) 3! ability distribution by a set of samples that match its moments. We show D△xf) that an approximation is only correct to the m th order if its moments f( E up to that order are correctly approximated Consider a Gaussian-distributrcd random variable x with mcan x (15) and covariance Par. We wish to calculate the mean y and covariance yy of the random variable y, which is related to x through the non- where ElDA-f(DAzf)']=Vf? has been used. Comparing linear(analytic)functiony=fx this with(2), it can be seen that the ekf truncates this series after the Noting that x can be written asx=x+x, where x is a zero-mean first term The new filter does not truncate the series at any arbitrary Gaussian random variable with covariance Paa, the nonlinear trans order, and, applying the analysis from above, it is correct up to the formation can be expanded as a Taylor Series about x second order with errors in the fourth and higher order terms. There fore, both the ekf and the new filter predict the covariance correctly y=fx+x up to the third order +DAf⊥Danf,Daf,D△rf APPENDIX III where the DAz f operator evaluates the total differential of f[ when THE MODIFIED FORM OF THE ALGORITHM perturbed around a nominal valuc x by x. The ith tcrm in the Taylor When g is negative, it is possible that the predicted covariance will series for given not be positive semidefinite. This can be demonstrated by taking the limit of (15) in Appendix l, the fourth and er order moments te fx lill Py=vf Paavfr-e E where Ti is the j th component ofx. Therefore, the i th term in the series is an i th-order polynomial in the coefficients of x, whose coefficients It can bc sccn that a fourth-ordcr, positivc-scmidcfinitc matrix is sub- are given by derivatives off tracted. Ilowever, this term originates from the outer product of two y is the expected value of (10) second-order expected terms and does not scale with hi y=fx+x The modified form of the algorithm evaluates the covariance ab f+E|D△f+Daf」DfD么f the projected mean. Taking the sigma points for x are t i and those for (12) y are i, the modified form for calculating Pyy is3 where the ith term in the series is given by MOD Wi[i Joli Jo]' 1 E his is equivalent to calculating the mean and covariance using(4)and(5) and adding a term y= Voly-VaT IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL 45, NO 3, MARCH 2000 Positive semidefiniteness is guaranteed by the fact that the covari- [13] P. S Maybeck, Stochastic Models, Estimation, and ControL. New ance matrix is evaluated as the sum of outer products of vectors It can York: Acdemic. 1979. voL. 1 be shown that the calculated covariance is [14]M. Athans, R. P. Wishner, and A. Bertolini, "Suboptimal state estima tion for continuous-time non linear systems from discrete noisy measure- ments, IEEE Trans. Automat Contr, vol 13. pp. 504-518, Oct 1968 P, - vir [15 D. E. Catlin. "Estimation, control and the discrctc Kalman filter, "in F「 DA f(Da f)⊥DfD3fT Applied Mathematical Sciences 71. New York: Springer-Verlag, 1989 2×2! [16]S. Clark, "Autonomous land vehicle navigation using millimetre wave Daf(D△f) radar, Ph D. dissertation, University of Sydney, Australia, 1999 3! [ D I erro and Y.K. Bar-Shalom, " Tracking with dehiased consistent con verted measurements VS EKF, TEEE Trans. Aerosp Electron. Syst. vol 29,pp.1015-1022,Juy1993 which introduces errors in the fourth and higher orders [18] J.K. Uhlmann, ""Simultaneous map building and localization for real The modified form has the advantage that, irrespective of the choic time applications, "Univ Oxford, U.K., Tech Rep, 1994 of the matrix square root [19 S J. Julier and J. K. Uhlmann, "A consistent, debiased method for converting between polar and Cartesian coordinate systems, "in Proc AeroSense: IIth Int. Symp. Aerosp / Defense Sensing, Simulat. Contr imy=f冈]+E DDA f Orlando FL. 1997 2! [20]SJ. Julier, "A skewed approach to filtering, " in Proc. Aero Sense: 12th lim Pyy=vfp Vf Int Symp. Aerosp. Defense Sensing Simulat. Contr, Orlando, FL, 199 Comprehensive process models for high-speed navigation Ph D dissertation, Univ Oxford, U. K,1997 These are the values calculated by the modified. truncated second [22]A. Montobbio, Sperimentazione ed affinamento di un localizzatore, B.S. thesis politechnico di torino Italy, 1998 rdr filter[4, but without the nccd to evaluate Jacobians or Hessians. 4[23]R Smith, "Navigation of an underwater remote operated vehicle, "Univ Oxford, Tech Rep, 1995 [24] T.S. Schei, "A finite difference approach to linearization in nonlinea estimation algorithms, "in Proc. Am. Contr: Conf, vol. 1, Seattle, WA ACKNOWLEDGMENT 1995,pp.114118 The authors would like to thank R. Bellaire at Georgia Tech for in [25R. L. Bcllairc, E.w.Kamen, and S M. Zabin, " A ncw nonlincar itcratcd filter with applications to target tracking, in Proc. Aero Sense: 8th Int sightful commcnts on an carly draft of this papcr and the rcfcrccs for Symp. Aerosp /Defense Sensing, Simulat. Contr, voL 2561, Orlando, FL their many constructive remarks 1995,pp.240-251 [26 B M. Quine, J.K. Uhlmann, and H. F. Durrant-Whyte, "Implicit Ja- cobians for linearized state estimation in nonlinear systems in Pmoc REFERENCES Amer: Contr: Conf, Seattle, WA, 1996, pp 1645-1646 [1]JK Uhlmann, Algorithms for multiple target tracking, "Am. Sci., vol 80,no.2.pp.128-141,1992 [2 IL. W. Sorenson, Kalman Filtering: Theory and Application. New ck: IEEE Press. 1985 [] H.J. Kushner, "Dynamical equations for optimum non-linear filtering, J Differential Equations, vol 3, pp 179-190, 1967 [4] P.S. Maybcck, Stochastic Models, Estimation, and Control, P.S. May- An O(T Boundary Layer in Sliding Mode for beck. Ed. New York: Academic. 1982. vol 2 Sampled-Data Systems [5]A. H. Jazwinski, Stochastic Processes and Filtering Theory. New York: Academic. 1970 6] N.J. Gordon, D J. Salmond, and A. F. M. Smith, "Novel approach Wu-Chung Su, Scrgcy V Drakunov, and Umit Ozguncr to nonlinear/non-Giaussian Bayesian state estimation, " Proc. Inst Elect Eng.F,Vol.140,no.2,pp.107-113,Apr.1993 "Approximations to optimal nonlincar filters, "IEEE Abstract-The use of a discontinuous control law(typically, sign func Trans. Automat. Contr, vol. AC-12, pp 546-556, Oct 1967 tions)in a sampled-data system will bring about chattering phenomenon [8]H. W Sorenson and A. R. Stubberud, " Non-linear filtering by approxi- the vicinity of the sliding manifold, leading to a boundary lay Ination of the a posteriori density, "Int J Contr, vol 8, no. 1, pp 33-51, thickness O(T), where T is the sampling period. However, by proper 1968 consideration of the sampling phenomenon in the discrete-time sliding [9] F. E. Daum, "New exact nonlinear filters, "in Bayesian Analysis of mode control design, the thickness of the boundary layer can be reduced Time Series and Dynamic Models, J C Spall, Ed. New York: Marcel to O(T). In contrast to discontinuous control for continuous-time VSS, Dekker.1988,p.199226 the discrete-time sliding mode control need not be of switching type [10]SJ Julier, J.K. Uhlmann, and H F Durrant-Whyte, "A new approach for filtering nonlinear systems, " in Proc. Am. Contr: Conf, Seattle, WA Index Terms-Boundary layer, discrete-time sliding mode, sampled-data 1995,pp.1628-1632 systems. [11]SJ Julier and J. K Uhlmann.(1994, Aug )A general method for ap proximating nonlinear transformations of probability distributions. [On- line].Availablehttp:/www.robots.ox.ac.uk/siju [2]—,“ A new extension of the Kalman filter to nonlinear systems,” Manuscript received April 3, 1998; revised August 31, 1998, and Februar in Proc. AeroSense: 11th Int. Symp. Aerosp / Defense Sensing, Simulat. 28, 1999. Recommended by Associate Editor,S. Hara. This work was Contr Orlando. FI.997 supported by the National Science Council of Taiwan, under Project NSC 89-2213-E-005-026 w.-C. Su is with the Department of Electrical Engineering, National Chung 4This can be contrasted with an alternative approach of the initial intuition, Hsing University, Taichung, Taiwan(e-mail: wcsu(@ dragon nchu.edu. tw which was explored in [26]. Under that scheme, no copies of the previously S. V Drakunov is with the Department of Electrical and Computer Engi estimated mean are included in the sample set and the sigma points are scaled neering, Tulane University, New Orleans, LA 70118 USA using a parameter a In the limit, as a tends to infinity, this algorithm predicts U. Ozguiner is with the department of Electrical Engineering, The ohio state the samc mcan and covariance as the EKF Howcvcr, when a= 1, this mcthod Univcrsity, Columbus, OH 43210 USA estimates the same mean and covariance as the new filter but withk-0 Publisher Item Identifier $0018-9286(00)02146-2 008_9286/00S10.00c2000IEEE 【实例截图】
【核心代码】