A contraction theory-based analysis of the stability of the Extended Kalman Filter
aa r X i v : . [ c s . S Y ] D ec A contraction theory-based analysis of the stability of theExtended Kalman Filter
Silv`ere Bonnabel ∗ Jean-Jacques Slotine † October 30, 2018
Abstract
The contraction properties of the Extended Kalman Filter, viewed as a deterministicobserver for nonlinear systems, are analyzed. This yields new conditions under whichexponential convergence of the state error can be guaranteed. As contraction analysisstudies the evolution of an infinitesimal discrepancy between neighboring trajectories,and thus stems from a differential framework, the sufficient convergence conditions aredifferent from the ones that previously appeared in the literature, which were derivedin a Lyapunov framework. This article sheds another light on the theoretical propertiesof this popular observer.
Keywords
Nonlinear asymptotic observer, Extended Kalman Filter, Contraction theory.
Since the seminal work of Kalman and Bucy [9] and Luenberger [14], the problem of buildingobservers for deterministic linear systems has been laid on firm theoretical ground. Yet, whenthe system is nonlinear, there is no general methods to tackle observer design. Over the lastdecades, nonlinear observer design has been an active field of research, and several methodshave emerged for attacking some specific nonlinearities. In the engineering world, the mostpopular method is the so-called Extended Kalman Filter (EKF), a natural extension of theKalman filter. The principle is to linearize the system around the trusted (i.e. estimated)trajectory of the system, build a Kalman filter for this time-varying linear model, and imple-ment it on the nonlinear model. The EKF is known to yield good results in practice whenthe guess on the initial state is close enough to the actual state, but possesses no guaranteeof convergence in the general case, and indeed can diverge.Since the 1990’s, several papers have addressed the convergence properties of the EKFviewed as a deterministic observer. Several conditions under which the estimation errorconverges to zero have been derived in, e.g., [1, 16, 2, 15]. In each case, a first set ofconditions on the observability and controllability of the system ensures the boundedness ofthe solution of the Riccati equation and of its inverse, and a second set of conditions ensurein this case the convergence of the estimation error to zero. Roughly speaking, the latterconditions require either the initial estimation error to be small, proving the EKF is a localobserver, or the system to be very weakly nonlinear. ∗ Robotics Center, Unit´e Math´ematiques et Syst`emes, Ecole des Mines ParisTech, 75272 Paris, France( [email protected] ). † Department of Mechanical Engineering, Massachusetts Institute of Technology, MA, 02139 USA( [email protected] ).
1n this paper, the convergence properties of the EKF are studied using contraction theory[12] and in particular the notion of virtual systems [17] and virtual observers [8]. Historically,ideas closely related to contraction can be traced back to [11, 7, 4] (see e.g. [13] for a moreexhaustive list of references). In the present case the idea is as follows: instead of studyingdirectly the evolution of the discrepency, in the sense of a Lyapunov function, between theestimated state and the true state, contraction theory allows to study the evolution of thediscrepancy between two nearby trajectories of the EKF, in the sense of a given metric. Itis shown that, in a finite region and under some conditions, two nearby trajectories tendexponentially towards each other. As a result, the EKF is a dynamical system which expo-nentially forgets its initial condition, a very desirable property for a filter. The fact that theestimation error tends exponentially to the true state appears then as a mere consequenceof the contraction properties of the filter. Even though the Lyapunov approach and thecontraction approach are based on very similar metrics, the convergence results obtained inthis paper differ from those of the literature.The main contributions of this paper are threefold. First, the paper studies the stabilityproperties of the EKF from the perspective of contraction theory. This offers an alternativeviewpoint to the usual Lyapunov approach, extending the preliminary results on linear time-varying systems of [8]. In turn, this perspective allows simple new convergence results to bederived in this context (see in particular Theorem 1 and its corollary). Finally, some of theresults are closely related to existing recent literature, showing both their similarities andtheir potential strengths.The paper is organized as follows. In Section 2, some new general contraction results arederived. Section 3 builds upon those results to derive bounds on the size of the contractionregion and the convergence rate of the EKF. Finally, Section 4 discusses some links withprevious work on the stability of the EKF.
Consider the following nonlinear deterministic system ddt x = f ( x, t ) (1) y m = h ( x, t ) (2)where x ∈ R n is the state, y m ∈ R p is the measured output, and f, h are smooth. The EKFequations are given by ddt ˆ x = f (ˆ x, t ) − P C (ˆ x, t ) T R − ( h (ˆ x, t ) − y m ( t )) (3) ddt P = A (ˆ x, t ) P ( t ) + P ( t ) A (ˆ x, t ) T + Q − P ( t ) C (ˆ x, t ) T R − C (ˆ x, t ) P ( t ) (4)where A ( x, t ) = ∂f∂x ( x, t ) , and C ( x, t ) = ∂h∂x ( x, t ). In the stochastic theory of Kalman linearfiltering, Q and R represent the covariances of the respectively drift noise and measurementnoise. In a deterministic and nonlinear setting as the one considered in the present paper, theycan be viewed more prosaically as design parameters where Q − represents the confidence inthe trusted model (1) and R − the confidence in the measurements (2). The present analysisrelies on the following assumption. Assumption 1
From now on we will systematically assume there exist p, p > pI ≤ P ( t ) ≤ pI . Moreover, for simplicity we assume that Q is fixed and invertible, and wedenote by q its smallest eigenvalue. 2he latter assumption on P ( t ) appears in most papers dealing with the stability of theEKF, e.g. [1, 5, 16]. It is well known that this assumption is verified as soon as the system˙ ξ = A (ˆ x ( t ) , t ) ξ, η = C (ˆ x ( t ) , t ) ξ is uniformly detectable. This is of course a very strongprerequisite on the behavior of the filter. Yet, note that this assumption can advantageouslybe checked by the user without any knowledge of the true trajectory. To the authors’ bestknowledge, very few papers have addressed the stability of the EKF without referring toAssumption 1: see [10] (and more generally high gain observers techniques [6]) where localconvergence results are derived under some different, yet rather restrictive, assumptions.Let K ( t ) = P C (ˆ x, t ) T R − denote the Kalman gain. Consider the “virtual” system [8] ddt z = f ( z, t ) − K ( t )( h ( z, t ) − y m ( t )) (5)The solution x ( t ) of the true system (1) is a particular solution of the virtual system, sincefor all t ≥ h ( x ( t ) , t ) − y m ( t ) = 0. The solution to the EKF equations (3)-(4) isobviously another particular solution of the virtual system. As a result, if it can be proventhe distance between two arbitrary trajectories of this system tends to zero, the convergenceof the estimation error ˆ x − x to zero will follow. In turn, this can be achieved by seekingconditions under which the virtual system (5) is contracting.Let us define a metric for the virtual system (5) by choosing, similarly to the lineartime-varying case considered in [8], the squared length k δz k P − = δz T P − δz (6)where P ( t ) is a solution of the Riccati equation (4) associated to the trajectory of the esti-mated state ˆ x ( t ). We have ddt ( δz T P − δz ) = ( ddt δz ) T P − δz + δz ˙ P − δz + δzP − ( ddt δz )= δz T [( A ( z, t ) − K ( t ) C ( z, t )) T P − + ˙ P − + P − ( A ( z, t ) − K ( t ) C ( z, t ))] δz Using the fact that ˙ P − = − P − ˙ P P − where ˙ P is given by (4), and that[ C ( z, t ) − C (ˆ x, t )] R − [ C ( z, t ) − C (ˆ x, t )]= C T ( z, t ) R − C ( z, t ) − C T (ˆ x, t ) R − C ( z, t ) − C T ( z, t ) R − C (ˆ x, t ) + C T (ˆ x, t ) R − C (ˆ x, t )(7)we finally have ddt ( δz T P − δz ) = δz T P − M P − δz (8)where M = P ˜ A T + ˜ AP + P ˜ C T R − ˜ CP − P C T R − CP − Q , where we let ˜ A ( z, t ) = A ( z, t ) − A (ˆ x, t ) and ˜ C ( z, t ) = C ( z, t ) − C (ˆ x, t ), and where C denotes the matrix C ( z, t ). Given two symmetric matrices P , P we define a partial order letting P ≤ P if P − P ispositive semidefinite. We have the following preliminary result: Lemma 1
Let ≤ γ < q/ (2 p ) . For each time t ≥ there exists r ( t ) > such that for all z satisfying k z − ˆ x ( t ) k ≤ r ( t ) we have P ˜ A T + ˜ AP + P ˜ C T R − ˜ CP ≤ Q − γP + P C T R − CP (9)3 roof The inequality is obviously verified for z = ˆ x as the right member of (9) is a positivedefinite matrix. As f, h are smooth, the inequality holds in a neighborhood of ˆ x .At any time, the vectors z lying within a distance at most r ( t ) of ˆ x ( t ) are contained inthe contraction region, as for those vectors equality (8) becomes the contraction inequality ddt ( δz T P − δz ) ≤ − γ ( δz T P − δz )This means the squared distance in the sense of metric (6) between two neighboring trajec-tories in this ball will tend to reduce, with a rate of change γ . This leads to the followinggeneral result: Theorem 1
Assume there exists < ρ = inf { r ( t ) , t ≥ } . Any trajectory of the system (5) that starts in the ball of center ˆ x (0) and constant radius ρ/ √ p with respect to the metric (6) remains in a ball of radius ρ/ √ p centered at the trajectory ˆ x ( t ) of the Extended KalmanFilter (3) - (4) , and converges exponentially to this trajectory in the sense of the metric (6) with a time constant /γ for the exponential decay. Mathematically, the theorem’s result can be expressed as follows. Let d P − be the geodesicdistance associated with the metric (6). Let z ( t ) , z ( t ) be the flows associated to the system(5) with initial conditions satisfying d P − ( z i (0) , ˆ x (0)) ≤ ρ/ √ p for i = 1 ,
2. Then for all times t ≥ d P − ( z ( t ) , z ( t )) ≤ e − tγ d P − ( z (0) , z (0)) (10) Proof
The theorem is a straightforward application of the Theorem 2 of [12] which statesthat any trajectory which starts in a ball of constant radius with respect to the metriccentered at a given trajectory and at all times in the contraction region with respect to themetric, remains in that ball and converges exponentially to this trajectory, which is a naturalresult in the theory of contracting flows (see e.g. [11]). Indeed, ( z − ˆ x ) T P − ( z − ˆ x ) ≤ ρ /p implies k z − ˆ x k ≤ ρ so z is contained in the contraction region by Lemma 1. Remark 1
Note that, if the system is linear, we have ˜ A ( z, t ) ≡ ˜ C ( z, t ) ≡ for all z, t , and werecover the fact that the deterministic Kalman filter for linear systems globally exponentiallyconverges under Assumption 1. The theorem implies an interesting result in the common case of linear output maps. Inmany nonlinear systems of engineering interest, the system output consists of an incompletemeasurement of the state vector. For instance, the output can be temperature or concentra-tions in chemical reactors, currents in induction machines, position or velocity in mechanicalsystems. Formally, this means that the output map is linear, i.e. h ( x ) = Cx , implying˜ C ( z, t ) ≡ z, t . Let then λ max ( · ) denote the largest eigenvalue of a symmetric matrix,and let γ ≥
0. The following result is an immediate consequence of Theorem 1.
Corollary 1
Assume that the output map is linear, and that λ max ( ˜ A ( z, t ) P ( t )+ P ( t ) ˜ A ( z, t ) T ) ≤ q − γp for all z, t . Then the Extended Kalman Filter (3) - (4) is globally exponentially con-vergent with a time constant /γ for the exponential decay. This result shows that contraction analysis can yield new types of conditions under whichexponential convergence of the EKF is guaranteed. Indeed, under the assumptions of Corol-lary 1, the EKF will converge globally without the standard requirement that the Hessian ofthe coordinates of f is uniformly bounded. However, in a more general context, this standardrequirement will still be needed as illustrated in the next section.4 A sufficient condition for exponential convergence
We now derive a lower bound on the size of the contraction region of Theorem 1. Thisresult relies on usual assumptions on boundedness of second derivatives ∂ f∂x , ∂ h∂x around theobserver trajectory ˆ x ( t ) , t ≥
0. We let k·k and ||| · ||| denote the norms on resp. matrices andtensors induced by the Euclidean norm on vectors.
Assumption 2
There are positive numbers α, κ A , κ C such that for all z satisfying k ˆ x − z k ≤ α and all t ≥ ||| ∂ f∂x ( z, t ) ||| ≤ κ A and ||| ∂ h∂x ( z, t ) ||| ≤ κ C .Under Assumptions 1 and 2, one can derive the following local exponential stability result: Corollary 2
Let γ ≤ q/ (2 p ) . Let ζ + be the positive root of the equation p r κ C ζ + 2 pκ A ζ − ( q − γp ) = 0 Let ρ = min( α, ζ + ) . Any trajectory of the system (5) that starts in the ball of center ˆ x (0) andconstant radius ρ/ √ p with respect to the metric (6) remains in a ball of radius ρ/ √ p centeredat the trajectory ˆ x ( t ) of the Extended Kalman Filter (3) - (4) , and converges exponentially inthe sense of the metric (6) with a time constant /γ for the exponential decay. In particular,this implies in the Euclidean metric on vectors, that k ˆ x ( t ) − x ( t ) k ≤ q p/p k ˆ x (0) − x (0) k exp − γt (11) for all t ≥ as soon as initially k ˆ x (0) − x (0) k ≤ ρ √ p/ √ p . Proof
The result directly follows from Theorem 1, as long as one can prove that ρ ≤ r ( t )for all t ≥
0, where r ( t ) is the radius of a ball around ˆ x ( t ) in which the matrix inequality (9)is verified. To begin, note that the ball of center ˆ x and radius ζ + is equivalently defined asthe set { z ∈ R n , pκ A k ˆ x − z k + p r κ C k ˆ x − z k ≤ q − γp } (12)Let ˜ e = z − ˆ x . As ˜ A ( z, t ) = A ( z, t ) − A (ˆ x, t ) = R ∂ f∂x (ˆ x + r ˜ e, t )˜ e dr and ˜ C ( z, t ) = C ( z, t ) − C (ˆ x, t ) = R ∂ h∂x (ˆ x + r ˜ e, t )˜ e dr , we have k ˜ A k ≤ κ A k ˆ x − z k and k ˜ C k ≤ κ C k ˆ x − z k . The largestvalue of the symmetric matrix ˜ AP + P ˜ A T satisfies λ max ( ˜ AP + P ˜ A T ) = max k w k =1 w T ( ˜ AP + P ˜ A T ) w = 2 max k w k =1 Trace( w T ˜ AP w ) ≤ p max k w k =1 w T ˜ Aw ≤ p max k w k =1 , k v k =1 | w T ˜ Av | ≤ p k ˜ A k ≤ pκ A k ˆ x − z k In the same way we have λ max ( P ˜ C T R − ˜ CP ) ≤ p r κ C k ˆ x − z k . Thus, as long as z belongsto the set (12), the inequality (9) is satisfied. 5 Links with previous work in the literature
The Extended Kalman filter has been shown to converge locally exponentially under a set ofconditions on the nonlinearities of the system, see e.g. [15, 5] in continuous-time and [16, 2, 1]in discrete-time. In these papers, the convergence analysis is based on the Lyapunov function V ( x − ˆ x, t ) = ( x − ˆ x ) T P − ( t )( x − ˆ x ) (13)At fixed time t , V ( x − ˆ x, t ) is the geodesic distance between x and ˆ x for the proposed metric k δz k P − = δz T P − δz Thus, it is no surprise that the convergence conditions derived in this article are very similarto those previously appearing in the literature. That said, the specificity of the contractionframework yields some differences that we shall detail in this section, which is organized asfollows. In Subsection 4.1, we emphasize a difference of point of view between contractionanalysis and Lyapunov based convergence analysis. In Subsection 4.2, we compare the con-vergence rate and basin of attraction of Section 3 with the results appearing previously in theliterature. For fair comparison we chose the article [15] which deals with the continuous-timecase. Finally, in Subsection 4.3, the modification of the EKF proposed in [15] is analyzedin the light of contraction theory, yielding a simple generalization of this work which isstraightforward in our framework.
In standard Lyapunov analysis of nonlinear deterministic observers, one generally seeks toprove the state error, i.e., the discrepancy between the estimated trajectory ˆ x and the truetrajectory x , measured by some Lyapunov function, tends to zero. Often the observers can beonly proved to be locally convergent, i.e., the initial guess ˆ x (0) must belong to some attractionbasin containing x (0). The contraction analysis of the present paper, based on the idea of virtual systems [17] and specifically virtual observers [8], builds upon a different approach.Indeed, the idea is to focus on a particular trajectory of the filter ˆ x ( t ) , t ≥
0, and then tostudy the evolution of two neighboring trajectories of the virtual system (5), and prove thatthe distance between them tends to shrink over the time. Under a set of conditions, we haveproved that this property holds in a ball centered at the particular trajectory ˆ x ( t ) , t ≥ x ( t ) belongs to thecontraction set or not.Besides, contraction theory provides a concrete result on the robustness of the EKFagainst external perturbations. Suppose indeed that x p ( t ) is some trajectory of a perturbed virtual system ddt x p = f ( x p , t ) − K ( t )( h ( x p , t ) − y m ( t )) + b ( x p , t ) where b ( x p , t ) represents adisturbance whose norm is supposed to be uniformly bounded by, say, k b k max . Then we have(see [12, 3]): k x p ( t ) − ˆ x ( t ) k ≤ q ( p/p ) (cid:0) e − tγ k x p (0) − ˆ x (0) k + γ k b k max (cid:1) . It proves that anytrajectory of the perturbed system converges exponentially to a ball of radius q ( p/p ) γ k b k max around the observer trajectory, allowing to evaluate the estimation error generated by theperturbation. The latter result completes the robustness properties of the EKF.6 onvergence rate Attraction basin for κ C = 0 Attraction basin for κ A = 0Lyapunov γ = qp/ (4 p ) k x (0) − ˆ x (0) k ≤ ( p/p ) [ q/ (4 κ A p )] k x (0) − ˆ x (0) k ≤ qr/ (4 cκ C p )Contraction γ = q/ (4 p ) k x (0) − ˆ x (0) k ≤ ( p/p ) / [ q/ (4 κ A p )] k x (0) − ˆ x (0) k ≤ ( qpr ) / / ( κ C p / √ First of all, to the authors’ knowledge Theorem 1 and its Corollary 1 have never appearedin the literature. The common approach is to study the evolution of the Lyapunov function(13) over time. While similar results to Theorem 1 and its corollary may possibly be workedout from this approach as well, they appear quite naturally in a contraction framework.Consider now the results of Section 3. As already mentioned, the standard Lyapunovfunction (13) represents the geodesic distance between ˆ x and x in the sense of the metricproposed in this paper. This is why the bounds derived in Section 2 are very similar tothose previously obtained in the literature. However, they are different, and this is mainlydue to the use of equation (7) in the result (8). Again, the analog of this transformation inthe Lyapunov framework is not easily seen, whereas it appears naturally in the contractionframework.Assume for simplicity that α = + ∞ . Table 1 compares the convergence rate and attrac-tion basin obtained in [15] and the ones obtained in the present paper in the two limitingcases κ A = 0 and κ C = 0. The results all correspond to the error equation (11) in the Eu-clidean metric. The rates and bounds in the Lyapunov approach of [15] have been obtainedletting α = 0 (this parameter has a different meaning in this article) and using the definitionof κ derived therein.We see that the results are quite similar, yet different. First of all, we immediatly seethat both the convergence rate and the size of the guaranteed attraction basin are larger inour approach in the limiting case κ C = 0, improving the formerly obtained bounds of [15].Indeed γ given in Table 1 is greater in the contraction case by a factor p/p ≥ p/p ) / ≥
1. The difference ismuch more remarkable in the limit case κ A = 0, in which case the size of the attraction basindepends heavily on the problem parameters. In particular, a noticeable difference is that thesize of the attraction basin in [15] depends on c , an upper bound on {k C ( t ) k , t ≥ } . Asa result, a large linearized observation matrix C ( t ) can diminish the guaranteed size of theattraction basin. On the other hand, the bounds obtained in the present paper do not relyon an upper bound c , and therefore they may yield stronger guarantees in some cases. In [15], the authors propose to modify slightly the EKF by adding a new term 2 βP in theRiccati equation (4), with β ≥
0, leading to a modified Riccati equation: ddt P = A (ˆ x, t ) P ( t ) + P A (ˆ x, t ) T + Q − P C (ˆ x, t ) T R − C (ˆ x, t ) P + 2 βP They prove the addition of such a term yields a faster convergence rate, as long as Assumption1 is preserved. Note that, such a term tends to increase the eigenvalues of P and thus todestabilize the Riccati equation. The fact that Assumption 1 remains then valid is thus nontrivial and must be checked. Then, as long as Assumption 1 is proved to hold, this termensures a faster convergence rate. This latter fact is easily understood in the contraction7ramework. Indeed, with this additional term, inequality (9) becomes: P ˜ A T + ˜ AP + P ˜ C T R − ˜ CP ≤ Q − γ + β ) P + P C T R − CP The benefits of this term are now obvious as it transforms the convergence rate γ into γ + β .In fact, contraction theory even offers a direct generalization of the work of [15]. Indeedconsider the following modified EKF observer ddt ˆ x = f (ˆ x, t ) − P C (ˆ x, t ) T R − ( h (ˆ x, t ) − y m ( t )) ddt P = A (ˆ x, t ) P ( t ) + P ( t ) A (ˆ x, t ) T + Q − P ( t ) C (ˆ x, t ) T R − C (ˆ x, t ) P ( t ) + 2 N where N is any positive definite matrix. It follows directly from equation (9) that as longas Assumption 1 still holds, the guaranteed convergence rate of this observer is increased byadding n/p , where n denotes the smallest eigenvalue of N . Acknowledgements
The authors would like to thank Laurent Praly for interesting feedback on the paper.
References [1] J.S. Baras, A. Bensoussan, and M.R. James. Dynamic observers as asymptotic limits ofrecursive filters: special cases.
SIAM J. Appl. Math. , 48:1147–1158, 1988.[2] M. Boutayeb, H. Rafaralahy, and M. Darouach. Convergence analysis of the extendedKalman filter used as an observer for nonlinear deterministic discrete-time systems.
IEEE transactions on automatic control , 42, 1997.[3] D. Del Vecchio and J.J. Slotine. A contraction theory approach to singularly perturbedsystems.
IEEE Trans. On Automatic Control . 2012. To appear.[4] B.P. Demidovich. Dissipativity of a nonlinear system of differential equations.
VestnikMoscow State University, Ser. Mat. Mekh. , 1961.[5] J. Deyst and C. Price. Conditions for asymptotic stability of the discrete minimum-variance linear estimator.
IEEE Trans. on Automatic Control , 13:702–705, 1968.[6] F. Deza, J-P. Gauthier and E. Busvelle. High-gain estimation for nonlinear systems.
Systems and Control Letters , 18:295–299, 1992.[7] P. Hartman. On stability in the large for systems of ordinary differential equations.
Canadian Journal of Mathematics , 13:480–492, 1961.[8] J. Jouffroy and J.J.E. Slotine. Methodological remarks on contraction theory. In , 2004.[9] R. Kalman and R. Bucy. New results in linear filtering and prediction theory.
BasicEng., Trans. ASME, Ser. D, , 83(3):95–108, 1961.[10] A. Krener.
Mathematical Systems Theory and Optimization , chapter The Convergenceof the Extended Kalman Filter, pages 173–182. Springer Verlag, 2002.811] D. C. Lewis. Metric properties of differential equations.
Amer. J. Math. , 71:294–312,1949.[12] W. Lohmiller and J.J.E. Slotine. On contraction analysis for nonlinear systems.
Auto-matica , 34(6):683–696, 1998.[13] W. Lohmiller and J.J.E. Slotine. Contraction Analysis of Nonlinear Distributed Systems.
International Journal of Control , 78(9), 2005.[14] D. Luenberger. An introduction to observers.
IEEE Trans. on Automatic Control ,16(6):596–602, 1971.[15] F. Sonnemann Reif, K. and R. Unbehauen. An EKF-based nonlinear observer with aprescribed degree of stability.
Automatica , 34:1119–1123, 1998.[16] Y.K. Song and J.W. Grizzle. The extended Kalman filter as a local asymptotic observer.
Estimation and Control , 5:59–78, 1995.[17] W. Wang and J.J.E. Slotine. On partial contraction analysis for coupled nonlinearoscillators.