Extended Kalman Filter Modifications Based on an Optimization View Point

Transcription

1 8th International Conference on Information Fusion Washington, DC - July 6-9, 05 Etended Kalman Filter Modifications Based on an Optimization View Point Martin A. Skoglund, Gustaf endeby Daniel Aehill Division of Automatic Control, Linköping University, SE Linköping, Sweden, {ms,hendeby,daniel}@isy.liu.se Dept. of Sensor & EW Systems, Swedish Defence Research Agency FOI, Linköping, Sweden Abstract he etended Kalman filter EKF has been an important tool for state estimation of nonlinear systems since its introduction. owever, the EKF does not possess the same optimality properties as the Kalman filter, and may perform poorly. By viewing the EKF as an optimization problem it is possible to, in many cases, improve its performance and robustness. he paper derives three variations of the EKF by applying different optimisation algorithms to the EKF cost function and relate these to the iterated EKF. he derived filters are evaluated in two simulation studies which eemplify the presented filters. I. INRODUCION he Kalman filter KF, [] is a minimum variance estimator if the involved models are linear and the noise is additive Gaussian. A common situation is however that dynamic models and/or measurement models are nonlinear. he etended Kalman filter EKF, [] approimates these nonlinearities using a first-order aylor epansion around the current estimate and then the time and the measurement update formulas as in the KF can be used. igher-order terms can be included in the aylor epansions if the degree of nonlinearity is high and thus not well approimated using only first-order information. Many suggestions have been made to improve the EKF. In the modified EKF [3] higher-order moments are included, [4, 5] use sigma-point approimations which is related to including second-order moments [6], while [7] proposes a robust EKF using theory. Another viewpoint is that the EKF is a special case of Gauss-Newton GN optimization without iterations, see e.g., [8, 9] for longer discussions. hus, iterations may improve the performance with just a little added effort. he idea eplored here is that it is not only linearisation errors that causes filter inconsistencies but rather the non-iterative way of including new information as it may not be sufficient for staying close to the true state. here is of course nothing that prevents the inclusion of higher-order terms when relinearising in the iterations. Notice that the Fisher scoring algorithm is also known as Gauss-Newton when applied to nonlinear least-squares see, e.g., [0]. An iterative version of EKF is the iterative etended Kalman filter IEKF, [] which re-linearises the measurement Jacobian at the updated state and applies the measurement update repeatedly until some stopping criterion is met. It should be noted that the iterations might not reduce the state error or reduce the associated cost function. It is actually not guaranteed even that a single update in the EKF improves the estimate since the full step may be to long. In [] a bistatic ranging eample illustrates when the IEKF converges to the true state as measurements become more accurate whereas the EKF is biased even though the state error covariance converges. Yet the EKF often works satisfactorily which can be understood in the light of being a GN method in which the first correction often gives a large improvement compared to the following, often smaller, corrections. Furthermore, the cost is often decreased for the full step. ence, both IEKF and EKF could easily be improved using step control using e.g., backtracking line search. hat is, with step control, or at least checking for cost decrease, unnecessary divergent behaviour of the EKF can be avoided. his paper shows that line search can be used in EKF and IEKF improve the overall performance. Furthermore, a quasi-newton approimation for EKF update with a essian correction term is derived. his matri can be successively built during the iterations using a symmetric rank- update. A slight modification of the Levenberg-Marquardt-IEKF LM- IEKF, [] is made to include a diagonal damping matri which could further speed up convergence. he paper is organized as follows. In Sec. II the cost function for the measurement update of the EKF is derived. Sec. III describes the EKF as an optimization problem via Gauss- Newton and introduces line search as a way to improve the convergence rate. he IEKF is further modified to include an eplicit step size. A quasi-newton EKF is the introduced and a slight modification of the LM-IEKF is then derived. Sec. IV contains two simulations eemplifying the derived filters and the papper ends with conclusions and future directions in Sec. V. II. E MEASUREMEN UPDAE AND IS COS FUNCION Consider systems having linear dynamics and nonlinear measurement models according to t = F t +w t y t = h t +e t a b where t is the state, y t the measurement, w t N 0,Q t and e t N 0,R t all at time t. In a Bayesian setting, the ISIF 856

2 Algorithm Kalman Filter Available measurements are Y = {y,...,y N }. Require an initial state, ˆ 0 0, and an initial state covariance, P 0 0, Q t and R t are the covariance matrices of w t and e t, respectively. ime Update Measurement Update ˆ t t = Fˆ t t P t t = FP t t F +Q t K = P t t P t t +R t, ˆ t t = ˆ t t +Ky t ˆ t t, P t t = I KP t t, 3a 3b 4a 4b 4c nonlinear Gaussian filtering problem propagate the following densities p t Y t N t ;ˆ t t,p t t a p t Y t N t ;ˆ t t,p t t b p t Y t N t ;ˆ t t,p t t c where p t Y t is the initial prior density, p t Y t is the prediction density, p t Y t is the posterior density and Y t = {y i } t i= are the measurements. he dynamic model b is linear which means that the prediction density is eactly propagated in the KF time update equations as given in Algorithm. he measurement update is however nonlinear and is given as the maimum a posteriori MAP estimate according to ˆ t t = argmap t Y t. 5 t he posterior is proportional to the product of the likelihood and the prior p t Y t py t t p t Y t ep yt h t R t yt h t +ˆ t t t P t t ˆ t t t, where terms not depending on t have been dropped. Since the eponential function is monotone and increasing we can minimise the negative log instead which gives where ˆ t t = argmin = argmin = argmin r = yt h R t yt h + ˆ t t P t t ˆ t t r r V 6 [ R / t ] y h, 7 P / t t ˆ t t and we have dropped the time dependence on the variable. he first term in the objective function is corresponding to the unknown sensor noise and the second term corresponds to the importance of the prior to the estimate. Furthermore, the optimization problem should return a covariance approimation ˆP t t. We simply base it on the assumption that the ˆ t t is optimal resulting in ˆP t t = I K i i ˆP t t, 8 since it should reflect the state uncertainty when 5 has converged. A general formulation for the MAP estimation problem is then {ˆ t t, ˆP t t } = argminv 9 where ˆP t t is computed using 8 while K i and i depends on the optimization method used. III. EKF VIEWED AS AN OPIMIZAION PROBLEM here is no general method for solving 6 and the nonlinear nature implies that iterative methods are needed to obtain an approimate estimate []. he Newton step p is the solution to the equation Vp = V. 0 Starting from an initial guess 0, Newton s method iterates the following equations i+ = i V i Vi, where V i and V i are the essian and the gradient of the cost function, respectively. Note that ifv is quadratic then gives the minimum in a single step. Gauss-Newton can be applied when the minimisation problem is on nonlinear least-squares form as in 6. hen the gradient has a simple form V = J r where and the essian is approimated as J = rs s, s= V J J, 3 avoiding the need for second-order derivatives. he GN step then becomes i+ = i J i J i J i r i. 4 where J i = J i and r i = r i are introduced to ease notation. Furthermore, the Jacobian evaluates to [ ] R / t i J i = where i = hs s, 5 s=i P / t t is the measurement Jacobian. 857

3 A. Line Search he EKF measurement update should ideally give a cost decrease V i+ < V i, 6 at each iteration. his will not always the case and this is why a full step in the EKF, and consequently in the IEKF, may cause problems. A remedy to this behaviour is to introduce a line search such that the estimate actually results in cost decrease. Without an eplicit step-size rule the EKF should be epected to have sub-linearly convergence [3] even though it should be linear [9]. A step-size condition was proposed in [8] for smoothing passes over the data batch 0 α i t c/t where c > 0 and α i t is initially small but should approach unity. here are many strategies for defining a sufficient decrease for the iteration procedure. he general iteration for Gauss-Newton with step-size parameter α is i+ = i α i J i J i J i r i, 7 where each α i is chosen as to satisfy some criterion e.g., the cost decrease condition 6. For the EKF update the GN step with step-size 7 evaluates to ˆ t t = ˆ t t +α i K y t hˆ t t 8 = ˆ t t +αp. 9 he cost decrease condition is then Vˆ t t +αp < Vˆ t t, 0 and it is only needed to find a suitable α over 0 < α starting with α =. Furthermore, an eact line search where α = argminvˆ t t +sp, 0<s is rather cheap since the search direction p is fied. A simple line search strategy can be to multiply the current step length with some factor less than unity until sufficient decrease is obtained. More advanced methods can of course also be used such as the Wolfe conditions, see e.g., [4], V i +α i p i V i +c α V i p i, V i +α i p i p i c V i p i, a b with 0 < c < c <, which also require that the slope has been reduced sufficiently by α i. he backtracking line search, see e.g., [5], is another effective ineact line search method which simply reduces the step by a factor α := αβ until V+αp < V+αγ V p, 3 is satisfied, starting with α =, with 0 < γ < 0.5 and 0 < β <. Algorithm Iterated Etended Kalman Measurement Update Require an initial state, ˆ 0 0, and an initial state covariance, ˆP Measurement update iterations i = hs 4a s s=i K i = ˆP t t i i ˆPt t i +R t 4b i+ = ˆ t t +K i yt h i i ˆ t t i 4c. Update the state and the covariance B. Iterated EKF ˆ t t = i+, ˆP t t = I K i i ˆP t t. 5a 5b he IEKF was derived in [9,, 6] and is summarised in Algorithm. he iterated update for the IEKF from 4c is i+ = ˆ+K i y t h i i ˆ i = i + ˆ i +K i yt h i i ˆ i 6 where ˆ = ˆ t t and the iterations are initialized with 0 = ˆ. With 7 the step parameter α can be introduced. he modified measurement update of the IEKF is then on the form i+ = i +α i ˆ i +K i yt h i i ˆ i = i +α i p i, 7 where the step length 0 < α i and the search direction p i is chosen such that 6, or a more sophisticated convergence criterion, is satisfied in each step. Note that for α i =, the first iteration is eactly the same as for the EKF. his step-size should always be attempted first since, if it is allowed, will result in faster convergence. We will refer to this method as IEKF-L. As a special case, the standard EKF is obtained in the first iteration of the measurement update 4. Note that the state covariance is updated using the last Kalman gain and measurement Jacobian when the iterations have finished 5 as discussed in 8. C. Quasi-Newton EKF Quasi-Newton type methods try to avoid eact essian and/or Jacobian computation for efficiency. he essian approimation described in 3 may be bad far from the optimum resulting in e.g., slow convergence. A quite successful approach to avoid this is to introduce a correction V i J i J i + i, 8 where i should mimic the dropped second-order terms. he step p with essian correction is the solution to the problem J J +p = J r 9 858

4 which is iteratively solved by i+ = i J i J i + i J i r i. 30 he state update for the quasi-newton EKF QN-EKF becomes i+ = ˆ+K q i yt h i i i S q i i i, 3a S q i = i R i i +P + i, 3b K q i = Sq i i R, 3c where P = P t t, h i = h i, i = ˆ i have been introduced. he derivation can be found in Appendi A. he matri i can be built during the iterations [7] using where i = i + w iv i +v i w i v i s i v i = J i r i J i r i w i s i v i s i v iv i, 3 = i R y h i + i R y h i, 33a z i = J i J i r i = i i R y h i, 33b s i = i i, w i = z i i s i. 33c 33d Furthermore, the quasi-newton relation i s i = z i is satisfied by construction. he iterations are initialized with 0 = 0 and i should be replaced with τ i i before the update where τ i = min, s i z i s i is i, 34 is a heuristic to ensure that i converges to zero when the residual vanishes. he covariance update with this approach can be updated using 8 for the last iterate ˆP t t = I K q i iˆp t t. 35 An eplicit line-search parameter can further be introduced i+ = i +α i i +K q i yt h i i i S q i i i. 36 using the same argumentation as in Section III-B. D. Levenberg-Marquardt Iterated EKF rust region methods for GN are similar to the essian correction above in the way that the Newton step is computed. he main difference being that only a single parameter is adapted at runtime. he well-known Levenberg-Marquardt method introduced by [8, 9] is such a method and it is obtained by adding a damping parameter µ to the GN problem Applying this to 4 results in J J +µip = J r. 37 i+ = ˆ+K i yt h i i i µi I K i i P i, 38a P = I P P + I P, 38b µ i K i = P i i P i +R, 38c which is referred to as Levenberg-Marquardt IEKF LM-IEKF. he damping parameter works as an interpolation between GN and steepest-descent when µ is small, and large, respectively. he parameter also affects the step-size, where large µ means shorter steps reflecting the size of the trustregion and it should be adapted at each iteration. he relations in 38 were derived in [] but, just as in [0], it was never indicated how µ is selected, in fact, they never seem to perform any iterations. herefore their solution rather works as ikhonov regularization for the linearized problem with fied µ. In [, Eample ] the damping have to be selected as µ = condj J 0 6 in order to obtain roughly the same update. he update, counterintuitively, has a larger cost, V, than the EKF update but a lower RMSE. his eample is etremely ill-conditioned and the essian approimation is thus not very useful. Furthermore, the condition number can serve as a simple heuristic for selecting the damping automatically resulting in condj J + µ I. Another option, due to [9], is to start with some initial µ = µ 0 typically µ 0 = 0.0 and factor v >. he factor is used to either increase or decrease the damping if the cost is increased or decrease, respectively. he LM-IEKF can be improved by replacing the diagonal damping with µdiagj J in 37 as suggested by [9]. By doing so, larger steps are made in directions where the gradient is small, further speeding up convergence. he resulting equations for the problem 6 then becomes i+ = ˆ+K i yt h i i i µ i I K i i PB i i, 39a P = I P P + B i P, 39b µ i K i = P i i P i +R, B i = diagji J i = diagi R i +P, which is shown in Appendi B. 39c 39d IV. RESULS he filters are evaluated in two simulations highlighting the effect of line search and the overall better performance obtained with the derived optimisation based methods in a bearings only tracking problem. A. Line Search o illustrate the effect of using line search in EKF and IEKF, consider the measurement function h =. 40 his result in a cost function with two minima in ± y and a local maimum at = 0 which makes the problem difficult. Fig. shows the estimates obtained with the EKF and IEKF initialized with 0 = 0., R = 0. and P = where V 0 = 4.90, while the true state is =, and a perfect measurement y = is given. In the IEKF the step length is reduced to α = 0.5 for the first iteration resulting in IEKF = 0.807, V IEKF = while the EKF equivalent to the IEKF 859

5 0 9 8 EKF IEKF - iteration IEKF - 3 iterations EKF.9 EKF IEKF-L LM-IEKF QN-EKF 7.8 V IEKF IEKF Fig.. EKF with full step increases the cost while half the step-size results in a reduced cost. Remember that EKF and IEKF are identical in the first step. hree IEKF iterations with line search significantly reduces the cost and further improves the estimate. without step-length adaption yields EKF =.5, V EKF = Another two iterations in the IEKF results in 3 IEKF = 0.978, V3 IEKF = wo things can be noted from this simple eample. First, the adaptive step-length improves the EKF noticeably at a relatively low cost. Second, the iterations in the IEKF further improves the estimate. Furthermore, with a smaller measurement covariance e.g., R = 0.0 the EKF performance degrades even more giving EKF = 4.0 not shown in Fig., while the IEKF performs well. his illustrates that EKF struggles with large residuals as the linearisation point differs much from its optimal value. B. Bearings Only racking he net eample, a bearings-only tracking problem involving several sensors, which is also studied in [, Ch. 5] and [, Ch. 4], is used to illustrate IEKF-L, QN-EKF and LM- IEKF. In a first stage the target with state = [X,Y] is stationary, i.e., w t = 0, at the true position = [.5,.5]. he bearing measurement function from the j-th sensor S j at time t is y j t = h j t +e t = arctany t S j Y,X t S j X +e t, 4 where arctan is the two argument arctangent function, S Y and S X denotes the Y and the X coordinates of the sensors, respectively. he noise is e t N 0,R t. he Jacobians of the dynamics and measurements are [ F = I, j Y S j = X S j X +Y S j Y ] Y X S j. X 4 With the two sensors having positions S = [0,0] and S = [.5,0]. he filters are initialised with ˆ 0 0 = [0.5,0.], P 0 0 = 0.I, R = π 0 5 I and Q = 0.I. Fig. shows the four filters posteriors with σ covariances after a single Y X Fig.. All filters ecept for the EKF are iterated 5 times with line search and come closer to the true state with a covariance that captures the uncertainty. ABLE I MONE CARLO SIMULAION RESULS FOR E FOUR FILERS. Method EKF IEKF-L LM-IEKF QN-EKF Mean-RMSE perfect measurement and 5 iterations. All filters, ecept for the EKF, result in acceptable estimates of the true target state and its uncertainty. After 0 iterations IEKF-L, LM-IEKF and QN-EKF produce nearly identical results. For the final eample 0 iterations are used to ensure the all filters have converged less would have sufficed and the result are clearly improved estimates compared to the EKF. A small Monte Carlo study is done with 0 trajectories generated by w t N 0,0.I, e t N 0,π 0 5 I for 0 time steps. he filters are initialized with the true position ˆ 0 0 = [.5,.5] with P 0 0 = 0.I. he results are summarized in able I where it can be seen again that all filters perform better than EKF with IEKF-L and LM-IEKF being slightly better than QN-EKF. V. CONCLUSIONS he cost function used in the etended Kalman filter EKF has been studied from an optimisation point of view. Applying optimisation techniques to the cost function motivates using an adaptive step length in the EKF and three iterated EKF variations are derived. In simulations it is shown that these filters outperform the standard EKF providing more accurate and consistent results. It remains to investigate the convergence rates obtained this way, as well as how to incorporate other EKF improvements such as the higher order EKF, square-root 860

6 implementations for improved numerical stability, and how to deal with colored noise. ACKNOWLEDGMEN his work was funded by the Vinnova Industry Ecellence Center LINK-SIC together with the Swedish Research Council through the grant Scalable Kalman Filters and the Swedish strategic research center Security Link. APPENDIX A DERIVAION OF E QUASI-NEWON EKF Using 7, 5 gives a simplified notation [ ] R r i = / y h i, 43 J i = P / ˆ i [ R / i P / he right-hand side of 30 evaluates to ]. 44 i Ji J i + i Ji r i = = i +i R i +P + i i R y h i +P ˆ i = ˆ+ i R i +P + i i R y h i i ˆ i i ˆ i = ˆ+S q i i R y h i i ˆ i S q i iˆ i = ˆ+K q i y t h i i i S q i i i, 45 which is the epression in 3 with i = ˆ i. APPENDIX B DERIVAION OF E MODIFIED LM-IEKF he LM-IEKF with modified step is evaluated using 37, 4, 43 and 44 as i Ji J i +µ i B i Ji r i = = i +i R i +P +µ i B i i R y h i +P ˆ i = ˆ+ i R i +P +µ i B i i R y h i i i µ i B i i = ˆ+K i y t h i i i µ i I K i i PB i i, 46 which is the epression in 39. Where we have used the relations and K i = i R i + P i R = P i i P i +R, 47 i R i + P = I K i i P, 48 P = P +µ i B i = I P P + B i P. 49 µ i REFERENCES [] R. E. Kalman, A new approach to linear filtering and prediction problems, ransactions of the ASME Journal of Basic Engineering, vol. 8, no. Series D, pp , 960. [] A.. Jazwinski, Stochastic Processes and Filtering heory, ser. Mathematics in Science and Engineering. Academic Press, Inc, 970, vol. 64. [3] N. U. Ahmed and S. M. Radaideh, Modified etended Kalman filtering, IEEE ransactions on Automatic Control, vol. 39, no. 6, pp. 3 36, Jun [4] S. Julier, J. Uhlmann, and. F. Durrant-Whyte, A new method for the nonlinear transformation of means and covariances in filters and estimators, IEEE ransactions on Automatic Control, vol. 45, no. 3, pp , Mar 000. [5] S. J. Julier and J. K. Uhlmann, Unscented filtering and nonlinear estimation, Proceedings of the IEEE, vol. 9, no. 3, pp. 40 4, Mar [6] F. Gustafsson and G. endeby, Some relations between etended and unscented Kalman filters, IEEE ransactions on Signal Processing, vol. 60, no., pp , Feb. 0. [7] G. A. Einicke and L. B. White, Robust etended Kalman filtering, IEEE ransactions on Signal Processing, vol. 47, no. 9, pp , Sep 999. [8] D. Bertsekas, Incremental Least Squares Methods and the Etended Kalman Filter, in Decision and Control, 994., Proceedings of the 33rd IEEE Conference on, vol., Lake Buena Vista, Florida, USA, dec 994, pp. 4. [9] P. J. G. eunissen, On the local convergence of the iterated etended kalman filter, in Proceeding of the XX General Assembly of the IUGG, IAG Section IV. Vienna: IAG, Aug. 99, pp [0] G. K. Smyth, Encyclopedia of Environmetrics. John Wiley & Sons, Ltd, 006. [Online]. Available: [] B. M. Bell and F. W. Cathey, he iterated Kalman filter update as a Gauss-Newton method, IEEE ransactions on Automatic Control, vol. 38, no., pp , Feb [] R. L. Bellaire, E. W. Kamen, and S. M. Zabin, New nonlinear iterated filter with applications to target tracking, in SPIE Signal and Data Processing of Small argets, vol. 56, San Diego, CA, USA, Sep. 995, pp [3]. Moriyama, N. Yamashita, and M. Fukushima, he incremental Gauss-Newton algorithm with adaptive stepsize rule, Computational Optimization and Applications, vol. 6, no., pp. 07 4, 003. [4] J. Nocedal and S. J. Wright, Numerical Optimization, nd ed. New York: Springer, 006. [5] S. Boyd and L. Vandenberghe, Conve Optimization. Cambridge University Press, 004. [6] Y. Bar-Shalom and X.-R. Li, Estimation and tracking: principles, techniques and software. Boston: Artech ouse, 993. [7] J. E. Dennis, D. M. Gay, and R. E. Welsh, Algorithm 573 NLSOL, An adaptive nonlinear least-squares algorithm, ACM ransactions on Mathematical Software, vol. 7, pp , 98. [8] K. Levenberg, A method for the solution of certain non-linear problems in least squares, Quarterly Journal of Applied Mathmatics, vol. II, no., pp , 944. [9] D. W. Marquardt, An algorithm for least-squares estimation of nonlinear parameters, SIAM Journal on Applied Mathematics, vol., no., pp , 963. [0] M. Fatemi, L. Svensson, L. ammarstrand, and M. Morlande, A study of MAP estimation techniques for nonlinear filtering, in Proceedings of International conference on information fusion FUSION, 03. [] G. endeby, Performance and implementation aspects of nonlinear filtering, Dissertations No 6, Linköping Studies in Science and echnology, Mar [], Fundamental estimation and detection limits in linear non- Gaussian systems, Licentiate hesis No 99, Department of Electrical Engineering, Linköpings universitet, Sweden, Nov