Left-invariant extended Kalman filter and attitude estimation

Transcription

1 Left-invariant extene Kalman filter an attitue estimation Silvere Bonnabel Abstract We consier a left-invariant ynamics on a Lie group. One way to efine riving an observation noises is to make them preserve the symmetries. We propose a leftinvariant (i.e, intrinsic an thus symmetry-preserving) extene Kalman filter such that the left-invariant estimation error obeys a stochastic ifferential equation inepenent of the system trajectory. The theory is illustrate by an attitue estimation example. I. INTRODUCTION Consier the problem of estimating a process x(t) from observation of a relate process y(t). This can be formulate in stochastic terms as a nonlinear filtering problem escribe by the stochastic ifferential equations ẋ = f(x, t) + M(x)w () y = h(x) + N(x)v () x(t ) = x () The state process an the output process evolve on n an p imensional manifols. x is gaussian, w an v are inepenent stanar white gaussian riving an observation noises. The nonlinear filtering problem is to compute in real time the conitional mean of the current state x(t) given the past observations {y(s), t < s < t}. The extene Kalman filter (EKF) yiels an approximation of the conitional mean escribe by a finite number of statistics. They evolve accoring to orinary ifferential equations (instea of stochastic partial ifferential equations) an thus can be compute in real time. Inee the Extene Kalman Filter is an observer, an it is the most wiely use non-linear filter. If we let A(t) = f h x (ˆx(t)) an C(t) = x (ˆx(t)) (an MMT, NN T represent the covariances matrices of the riving an observation noise) the EKF equations write ˆx = f(ˆx, t) + K(t)(y h(ˆx)) t (4) K(t) = P (t)c T (t)(nn T ) (5) t P (t) = A(t)P (t) + P (t)at (t) + MM T (6) P (t)c T (t)(nn T ) C(t)P (t) The principle is to buil a Kalman filter for the linear approximation of ()-()-() an implement it on the nonlinear moel. The EKF system oes not take into account the specific geometry of the ynamics moel (). When the moel amits symmetries, the EKF equations o not respect these Centre Automatique et Systèmes, École es Mines e Paris, 6 boulevar Saint-Michel, 757 Paris CEDEX 6, FRANCE silvere.bonnabel@ensmp.fr symmetries. Usually symmetries are erive from physical consierations (inepenence of physical units, invariance by a change of coorinates...) an we woul like the observer to respect these symmetries. In this paper, for the sake of simplicity, we will consier the (simple) case where the ynamics f amits n symmetries. This means a n- imensional lie group acts on a n-imensional state space. Uner some assumptions on the regularity of the group action it is equivalent to consier x evolves on a Lie group an ẋ = f(x, t) is a left-invariant ynamics. In this case we will see how to efine the noises M(x)w an N(x)v so that (i) they preserve the symmetries (ii) the EKF takes into account the specific geometry of the system an preserves the symmetries. Our observer is given by ()-()-()-(4). As far as we know such left-invariant (an thus intrinsic) formulation of an extene Kalman filter is new. The iea of the intrinsic EKF is the following. The noises epen on the state variables in such a manner that the two following methos are equivalent: ) Take the non-linear observer ()-()-()-(4); consier the well-pose error (i.e, formulate intrinsically) between the estimate system an the true system with the noise turne off; compute its time erivative; linearize it; a aitive noises to the linearize system, make a Kalman Filter for this system (first-orer error equation); erive the gain K of the observer ) take the ynamics (7)-(8)-(9) where the noises are left invariant; compute the well-pose error equation; linearize it; make a Kalman filter for the linearize error an erive the gain K. There have been several attempts to introuce geometry in the problem of nonlinear filtering ([5], [], [7]). Builing observers which respect the geometry of the system have been one in [4], [], [], [9], [8], [6], []. The contribution of this paper is to make an intrinsic extene Kalman filter for a nonlinear left-invariant ynamics on a Lie group. The paper oes not exhibit any convergence result. We only eal with the structure of the observer. Results about convergence of the EKF can be foun, e.g., in []. Results about the convergence of left-invariant observers on a Lie group can be foun in [4]. In section II we show how to efine the noises which respect the geometry of the system. In section III we exhibit a left-invariant extene Kalman filter on a Lie group. In section IV we present a a simplifie example of inertial navigation (only attitue estimation) also consiere in [8]. Simulations are mae in section V.

2 II. SYMMETRIES AND NOISES From now on we will consier the ynamics ẋ = f(x, t) + M(x)w (7) y = h(x) + N(h(x))v (8) x(t ) = x (9) where x lies in a n-imensional real Lie group G, y R p, x is gaussian, w an v are inepenent stanar white gaussian riving an observation noises, M(x) is a morphism which epens on the parameter x an maps the tangent space at the ientity element e of the group to the tangent space at x. N(h(x)) is an operator of R p. Moreover we will assume the moel is such that f is a left invariant vector fiel: for all g, x G f(l g x, t) = DL g f(x, t) () where L g (x) = gx enotes the left multiplication on G an (as in the sequel) D stans for ifferentiation. The property also reas: for any g G the system is left unchange by the transformation Z = L g (z). Inee () implies Ż = f(z, t). A goo choice of the noise matrices when one eals with applications can be ifficult to fin. A logical way to efine the noises in our case is to use geometrical consierations. A. Invariant riving noise Consier the moel ynamics with noise turne off ż = f(z, t). We will look at w as an aleatory element of the Lie algebra G of G, i.e, the tangent space to the group ientity element e. To efine a noise which preserves the symmetry we will assume M(x) is efine by M(x) = DL x M(e) where M(e) is any enomorphism of G. It implies for any g G, DL g M(x) = M(L g (x)) since DL g DL x = DL gx. So the ynamics (7) affecte by white noise is also invariant. Inee let X = L g (x) we have Ẋ = DL g ẋ = DL g (f(x, t) + M(x)w) = f(l g (x), t) + DL g M(x)w = f(x, t) + M(X)w which proves invariance of the ynamics (7). M(e) represents the noise magnitue when x is close to e. B. Invariant observation noise We suppose that the output map h : G R p preserves the symmetries in the sense that it is G-equivariant (as in [], [], [4]). This means there exists a set of iffeomorphisms ρ g parameterize by the group elements g such that for any g, g G ρ g ρ g = ρ g g an for all g, x G h(l g x) = ρ g h(x). As in usual nonlinear filtering (equation ()), we consier the output y to be the measurement of h(x) affecte by an aitive white gaussian noise (ue for example to the imperfections of the sensors). If the ynamics is invariant uner a certain transformation (change of coorinates etc.) we want the observation noise to preserve this invariance in the following sense: N(h(x)) is a matrix epening on h(x) which verifies for all x, g G N(ρ g (h(x))) = Dρ g N(h(x)) where Dρ g enotes the tangent map inuce by ρ g. Thus the output Y at X = L g (x) is a function of g, h(x) an the noise N(h(x))v Y = h(x) + N(h(X))v = ρ g (h(x)) + Dρ g N(h(x))v Moreover, we assume that the iffeomorphisms ρ g are linear, as in the example of section IV, as well as in the examples treate in [4], [], [], [8], [6]. Thus ρ g (h(x)+n(h(x))v) = ρ g (h(x)) + Dρ g N(h(x))v an Y = ρ g (y) where we recall Y is the output at X = L g (x). It means observation noise is such that the output oes not estroy the symmetries in the sense it is G-equivariant (in other wors compatible with the group action via ρ g ). III. INTRINSIC EXTENDED KALMAN FILTER Consier the ynamics (7)-(8)-(9). Let A(t) : G ξ [ξ, f(e, t)] G N = N(h(e)) C = Dh(e) M = M(e) where e enotes the group ientity element an [, ] the Lie bracket associate to the Lie algebra G. Consier the observer ˆx = f(ˆx, t) + DLˆx K(t) ( ρˆx (y) ρˆx (h(ˆx)) ) () K(t) = P (t)c T (t)(nn T ) () P (t) = A(t)P (t) + P (t)a T (t) + MM T () P (t)c T (t)(nn T ) C(t)P (t) P () = P (4) This observer is a sort of extene Kalman filter for (7)- (8)-(9) when riving an observation noises are chosen as explaine in section II. Moreover it possesses the same symmetries as the system (7)-(8)-(9), i.e, it is left-invariant. Contrarily to the usual EKF the gain K(t) epens on t but oes not epen on the estimation ˆx(t)! It remins the linear nonstationary case. It can be compute off-line. The form of the observer is inspire from [4], []. A. The observer is symmetry-preserving An observer ˆx = F (ˆx, y) for (7)-(8)-(9) is sai to be invariant (i.e, symmetry-preserving) (see [4], []) if it respects the geometry of the system in the following sense: for any g G if we let Y = ρ g (y) an ˆX = L g (ˆx) we have ˆX t = F ( ˆX, Y ). We are going to prove that the observer we propose is invariant by left multiplication. Inee consier now (). For any g G if we let L g (x) = gx = X an Y = ρ g (y) we have t ˆX = DL g f(ˆx, t) + DL g DLˆx K(t) ( ρˆx (y) ρˆx (h(ˆx) ) = f(gˆx, t) + DL gˆx K(t) ( ρ (gˆx) (ρ g (y)) ρ (gˆx) (h(gˆx) ) = f( ˆX, t) + DL ( ) ˆXK(t) ρ ˆX (Y ) ρ ˆX (h( ˆX)

3 since DL g f(g, t) = f(g g, t), ρ (gˆx) = ρˆx ρ g, h(gˆx) = ρ g h(ˆx). B. The observer is an intrinsic Extene Kalman Filter The iea of the EKF is to linearize equations (7)-(8) aroun the trajectory ˆx(t), then buil a Kalman filter for the linear moel an implement it on the non linear moel. Then one can tell P is an approximation of E((ˆx x)(ˆx x) T ). The iea of the intrinsic EKF for a left-invariant ynamics is to compute the time erivative of an intrinsic error, then linearize it consiering the error is small, an buil a Kalman filter for the linear error equation an implement it on the non linear moel. ) Intrinsic error: The usual state error x = ˆx x oes not have any geometrical sense for x, ˆx G. One shoul rather use the left invariant equivalent state error η = x ˆx so that ˆx = xη an not ˆx = x + x. This error is intrinsic in the sense that it is invariant uner a left multiplication. Inee for g G we have (gx) (gˆx) = x ˆx. When the observer is efine by ()-()-()-(4), inspiring from [4] we have the following error equation t η = DL ηf(e, t) DR η (f(e, t) + M(e)w) + DL η K ( h(η ) + N(h(η ))v h(e) ) (5) where R η enotes the right multiplication by η on G. The computations are analogous to those etaile in section IV. A remarkable feature is that the non linear error equation only epens on the noises w an v, the time t, an η itself. It is thus inepenent of the trajectory an only epens on the relative positions of ˆx an x, as in the linear case. ) Linearize error equation: We want to make a Kalman filter for the linear moel for the error. For a small error, i.e, η close to e, one can set η = exp(ξ) where ξ is a small element of the Lie algebra G. Up to orer terms in ξ, we have the following linearize invariant state error equation on G ξ = [ξ, f(e, t)] (I + r(ξ))m(e)w t + (I + l(ξ))k ( (I (ξ))n(h(e))v Dh(e)ξ ) (6) where I is the ientity operator, l, r an are the erivative at the ientity of the group of DL, DR an Dρ: l(ξ) = s DL exp(sξ), r(ξ) = s DR exp(sξ) an (ξ) = s Dρ exp(sξ). Such a stochastic ifferential equation is calle a multiplicative equation. A treatment of multiplicative equations can be foun in []: Let α be a parameter measuring the magnitue of the noises. Consier the solution of the following equation (7). Its mean an its covariance matrix are solutions to orer α of the ifferential equations verifie by the mean an the covariance of the solution ξ of (6). Thus: if the observer mae for (7) is robust, the expectation of ξ (verifying (6)) will also ten to as t ; an the covariance matrix of the solution of (7) will be an approximation of the covariance of the solution of (6) to secon orer in α. Thus the solution of (7) yiels an approximation of (6). The linearize error equation can be approximate now by the error equation: ξ = [ξ, f(e, t)] M(e)w + K (N(h(e))v Dh(e)ξ) t (7) when A, C, M an N are efine as above, this equation can be re-written ξ = (A KC)ξ Mw + KNv (8) t which is the usual linear equation for the error when one makes a linear Kalman filter. A Kalman filter for the linear moel is given by equations (8) an ()-()-(4) where P enotes the (intrinsic) covariance matrix P (t) = E(ξξ T ) of the linearize error. Thus ()-()-()-(4) can be looke at as a left-invariant intrinsic EKF. A. Quaternions IV. EXAMPLE A quaternion p H can be viewe at a set of a scalar p R an a vector p R, ( ) p p =. p The quaternion multiplication writes ( ) p q p q := p q. p q + q p + p q ( The ientity element is e := ) an (p q) = q p. Any vector ( p R can be looke at as a quaternion p := an we will systematically ientify vectors p) p R with the corresponing quaternion p H. To any quaternion q whose norm is one can associate a rotation matrix R q SO() the following way: q p q = R q ( p) for all p. A motion on the group of rotations of three-imensional eucliian space can be written thanks to quaternions or rotation matrices. The use of quaternion is stanar in inertial navigation problems. Quaternions with norm are suite to computer simulations because it is easier to maintain numerically the norm of q equal to rather than a rotation matrix in SO(). B. An attitue filtering problem The motion equations of kinematics for a flying rigi boy write (using quaternions H) q = q ω + q Mw (9) where q H is the quaternion of norm one which represents the rotation which maps the earth frame to the boy frame, ω(t) is the instantaneous angular velocity vector measure by gyroscopes, M is a constant matrix an w is a stanar gaussian white noise of R, is the non commutative quaternion multiplication.

4 The measurements are the instantaneous rotation vector ω(t), as well as the magnetic fiel an the specific acceleration. The two last measurements (mae by magnetometers an accelerometers) are combine together to provie an algebraic estimation q y of the attitue q. This approximation is vali when the acceleration of the boy is small compare to the magnitue of the earth magnetic fiel ( m.s ). The metho to get q y is explaine in [8] which consiers the same ynamics (9) with the same output q y but without any noise (M = N = ). We consier that the riving noise q Mw represents the error mae making this approximation (more practically it means that we suppose error sources arising from gyro can be moelle white noise). The output of the system is q y = q + q Nv where v is a stanar gaussian noise of R which correspons to the imperfections of the sensors, an N a real matrix. C. Symmetries of the system The kinematics equations (9) are inepenent of the choice of the earth frame. Notice there is no reason why the riving an observation noise shoul epen on the coorinates either. They were chosen so that they respect this invariance uner a change of coorinates. We want to make an intrinsic extene Kalman filter for this problem, i,e, which oes not epen on the choice of the earth frame an hence preserves the symmetries which is an intrinsic property. Let us verify the ynamics is a left-invariant ynamics on a Lie group. The subgroup of quaternions with norm is a Lie group. Let r be any quaternion with norm. Make the left multiplication Q = r q. The equations for Q are similarly written Q = r q ω + r q Mw = Q ω + Q Mw Moreover the output is compatible since Q y = r q y = r (q + q Nv) = Q + Q Nv The physical sense is the following : r is a quaternion whose norm is so it represents a rotation an Q = r q represents the attitue quaternion after having mae the earth frame rotate. The ynamics an output equation correspon to (7)- (8) when noises are chosen as in section II. D. An intrinsic extene Kalman filter Instea of consiering the usual linear error ˆq q we consier the equivalent intrinsic error η = q ˆq (see section III-B.). η is a quaternion which correspons to the rotation which maps the true frame to the estimate frame an hence oes not epen on the choice of the earth frame. P() is a efinite positive matrix representing the initial covariance of the error E(ηη T ). The commutator p (p ω(t) ω(t) p) is a linear function of the quaternion p we let A(t) enote. Consier the following observer (corresponing to () -()- ()): t ˆq = ˆq ω + ˆq K(t)(ˆq q y ) K(t) = P (t)(nn T ) P (t) = A(t)P (t) + P (t)a T (t) + MM T P (t)(nn T ) P (t) () The gain P(t) is calculate so that the tangent approximation of the error equation verifies the linear Kalman filter equations. Inee the estimation error equation has the following ynamics (corresponing to (5)): η = (η ω ω η) Mw η + η K(η + η Nv) since t q = q q q = ω q an ˆq q = η. Let us assume the ˆq is close to q an set η = + δη with δη a small quaternion whose first coorinate is equal to,i.e δη belongs to the tangent space at the ientity of the subgroup of quaternions whose norm is. We have up to secon orer terms in δη δη = A(t)δη Mw Mw δη δη KNv + KNv Kδη Let us suppose the noise Mw an Nv are small enough so that Mw δη an δη KNv are secon orer terms. If the noises are too big, there shoul be no reason why η can remain small. The linear approximation of the error equation becomes (corresponing to (7)) : δη = A(t)δη Kδη Mw + KNv Thus filter () is an intrinsic observer such that the gains correspon to a Kalman filter built for linear approximation of the error. This is the same principle as the usual EKF but here the error is expresse in an intrinsic way. As an interesting by-prouct, one can notice the error equation oes not epen on ˆq(t) ([4], []) an thus the convergence (an the tuning of the gains i,e, the choice of M an N) oes not epen on the trajectory although the ynamics is nonlinear. V. SIMULATIONS To get realistic values of ω, an q y we generate a trajectory of a VTOL-type rone which the same as the one in [4]. The center of mass of the flying boy follows a circle whose raius is 5 meters an stops. The following high frequencies are ae to signals corresponing to the smooth trajectory of the VTOL-type rone ω(t) an q(t) in orer to represent the imperfections of the sensors : q y (t) = q(t) ( +. σ ) an for the measure ω(t) we take ω(t) +.5 σ, where the σ i are inepenent normally istribute ranom -imensional vectors with mean an variance. The amplitue of the noise is % of the maximal value of the variables.

5 The initial conitions are such that the initial rotation iffers from the true one up to a π/ angle. It writes for the corresponing quaternions: q() = [,,, ] ˆq() = [cos(π/), sin(π/)/ (), sin(π/)/ (), sin(π/)/ ()] We take for the noises matrices : M =.5 I an N =. I an P () =. I (for instance) where I is the ientity matrix. The measurements an the behavior of ˆq an q (as well as the estimation error) are represente by figures an. Figure shows the error convergence is the same when we take the same ω(t) an the same initial error η() but a ifferent initial q()..5.5 Measure of the attitue q y qy qy qy qy Fig.. Iem: four coorinates of attitue q (soli line) an estimate attitue ˆq (ashe-line); estimation error E Q = q ˆq using leftinvariant EKF (). The only ifference with Fig is that the initial conition q() = [.5,.5,.5,.5] is ifferent, but η() is the same. Although the trajectory q(t) is ifferent, the error E Q has the same behavior. Q Q Q Q 4 4 E Q P 4 Angular velocity in the boy frame ω m ω m ω m P Fig.. Measure noisy signals : attitue q y an angular velocity ω (ra/s) in the boy frame..4.. P Fig.. Four coorinates of attitue q (soli line) an estimate attitue ˆq (ashe-line); estimation error E Q = q ˆq using left-invariant EKF (). VI. CONCLUSION The intrinsic EKF has two practical avantages : a systematic way of choosing noises hence of tuning the gains, an Q Q Q Q 4 4 E Q Fig. 4. Diagonal coefficients of the matrix P. Every parameter is the same as in section V except that here we took P () =. I to illustrate the convergence of the iagonal coefficients. the error equation (hence the assignment of the eigenvalues) oes not epen on the trajectory x(t) but only on the relative positions of x an ˆx, although the ynamics is not linear. One can also consier the general case of a ynamics on a n-imensional manifol invariant uner the action of a r- imensional Lie group with r < n, i.e, there are only r symmetries generate by local group actions. In this case it is also possible to efine an invariant state-error [4] an to make an symmetry-preserving extene Kalman filter in a similar way to the one evelope in this paper. REFERENCES [] N. Aghannan an P. Rouchon. On invariant asymptotic observers. In Proceeings of the 4st IEEE Conference on Decision an Control, volume, pages ,. [] N. Aghannan an P. Rouchon. An intrinsic observer for a class of lagrangian systems. IEEE AC, 48(6):96 945,.

6 [] S. Bonnabel, Ph. Martin, an P. Rouchon. A non-linear symmetrypreserving observer for velocity-aie inertial navigation. In American Control Conference (ACC6), pages 9 94, June 6. [4] S. Bonnabel, Ph. Martin, an P. Rouchon. Symmetry-preserving observers. Accepte for publication in IEEE AC, Dec 6. [5] R.W. Brockett. Remarks on finite imensional nonlinear estimation. Asterique, 75-76:47 55, 98. [6] T Hamel an R. Mahony. Attitue estimation on so() base on irect inertial measurements. In International Conference on Robotics an Automation, ICRA6, 6. [7] A. Krener. Algebraic an Geometric Methos in Nonlinear Control Theory, chapter The intrinsic geometry of ynamic observations, pages D.Reiel Publishing Company, 986. [8] R. Mahony, T. Hamel, an J-M Pflimlin. Complimentary filter esign on the special orthogonal group so(). In Proceeings of the IEEE Conference on Decision an Control, CDC5, Seville, 5. [9] D. H. S. Maithripala, W. P. Dayawansa, an J. M. BERG. Intrinsec observer-base stabilization for simple mechanical systems on lie groups. SIAM J. Control an Optim., 44:69 7, 5. [] S.I. Marcus. Algebraic an geometric methos in nonlinear filtering. SIAM J. Control Optimization, :87 844, 984. [] Y.K. Song an J.W. Grizzle. The extene kalman filter as a local asymptotic observer. Estimation an Control, 5:59 78, 995. [] N.G Van Kampen. Stochastic process in physics an chemistry. Elsevier, North Hollan Personal Library, 99.