Invariant Extended Kalman Filter: Theory and application to a velocity-aided estimation problem

Transcription

1 Invariant Extene Kalman Filter: Theory an application to a velocity-aie estimation problem S. Bonnabel (Mines ParisTech) Joint work with P. Martin (Mines ParisTech) E. Salaun (Georgia Institute of Technology) Shanghai, 16th ec 29

2 Introuction Symmetries were use in control for feeback esign but much less for observer esign. When a system possesses symmetries, the stanar extene Kalman filter generally oes not preserve the symmetries. For a non-linear system possessing symmetries, aitive white noise oes not preserve the symmetries.

3 The extene Kalman filter (EKF) The system is efine by a stochastic ifferential equation, ẋ = f (x, u) + M(x)w y = h(x, u) + N(x)v, where x, u, y belong to an open subset of R n R m R p ; w, v are white gaussian noises. Stanar EKF equations: ˆx = f (ˆx, u) + K (y h(ˆx, u) ) = F(ˆx, u, y) K? Compute the gain K as in a linear Kalman filter since the estimation error x = ˆx x satisfies up to higher orer terms the linear equation ẋ = (A KC) x Mw + KNv. (1) A = 1 f (ˆx, u), C = 1 h(ˆx, u), K = PC T (NN T ) 1 Ṗ = AP + PA T + MM T PC T (NN T ) 1 CP, What about this linear" approach when the state space is a manifol, a group??

4 An example: GPS-aie inertial navigation The motion of a rigi boy is where R = R(ω ) t t V = A + Ra R SO(3) is the orientation of the boy mapping the boy frame to earth frame V is the velocity with respect to earth frame ω is the angular velocity measure by gyros a is the specific acceleration measure by acceleros A = ( g) T is the constant gravity vector in North-East-Down (NED) coorinates Quaternions are well suite to calculations an computer implementation

5 Use of the quaternions p = ( ) p p ( R 3 p p) H Multiplication law : p q := ( ) p q p q. p q + q p + p q Unit element : e := ( 1 ), To any quaternion q whose norm is 1, we can associate a certain rotation matrix R q SO(3) thanks to the following formula q 1 p q = R q p pour tout p.

6 The consiere system: GPS/IMU fusion To esign our observers we consier the system q = q (ω m ω b ) V = A + 1 a s q a m q 1 ω b = ȧ s =, where ω m an a m are seen as known inputs, together with the output ( ) ( ) yv V = q 1. B q y B where B is the constant earth magnetic fiel, measure by magnetometers.

7 The multiplicative extene Kalman filter (MEKF) The linear error q = ˆq q oes not have much sense for quaternion. The EKF upate oes not preserve ˆq = 1. Well-known MEKF 1 base on the group error q 1 ˆq with t ˆq = ˆq (ω m ˆω b ) + ˆq K q E t ˆV = A + 1 â s ˆq a m ˆq 1 + K V E t ˆω b = K ω E, t âs = K a E. ) (ŷv y E = V. ŷ B y B 1 E. Lefferts, F. Markley, an M. Shuster, Kalman filtering for spacecraft attitue, Y. Huang, F. Chang, an L. Wang, The attitue etermination algorithm using integrate GPS/INS ata, IFAC 25.

8 The multiplicative extene Kalman filter (MEKF) Let us suppose the noise enters the system as t q = q (ω m ω b ) + q M q w q t V = A + 1 a s q a m q 1 + q M V w V q 1 t ω b = M ω w ω t a s = M a w a, an the output as ( yv y B ) ( = V + N V v V q 1 B q + N B v B ), with M q, M V, M ω, N V, N B iagonal matrices. The riving an observation noises are thus consistent with a scalar aitive noise on each iniviual sensor.

9 The multiplicative extene Kalman filter (MEKF) Tuning? Matrices A, C? The state error µ = q 1 ˆq, ν = ˆV V, β = ˆω b ω b an α = â s a s yiels the error system aroun (µ, ν, β, α) = (1,,, ): δ µ δµ w q ( ) δ ν δ β = (A KC) δν δβ M w V w ω + KN vv, v B δ α δα w a which has the esire form with A, C, M, N epening on ˆq, ω m, a m.

10 Features of the MEKF Soun geometric structure for the quaternion estimation equation by construction it preserves the unit norm of the estimate quaternion. Driving noise is a sensor noise. Possible convergence issues in many situations. Inee, the matrices A an C use for computing the gain matrix K are constant only in level flight.

11 Invariant Extene Kalman filter Provies a geometric framework to the MEKF. We notice the state space is a group G for the law given by p q p q V ω V ω b := p (V + V ) p 1 ω b + ω, a a s a s a The physical meaning is clear: rotation an translation in Earth axes, translation in boy axes, an scaling. We also consier the group transformation ω m ω m + ω ψ (p a m,v,ω,a ) A = a a m p A p 1 B p B p 1 ρ (p,v,ω,a ) ( yv y B ) = ( p (y V + V ) p 1 y B ).

12 Invariant Extene Kalman filter Let g = (p, V, w b, a s ) T G enote the state. The system with noise turne off writes g = f (g, u) t y = h(g, u) with u = (w m, a m, A, B) T. Let g = (p, V, w, a ) T. The system is invariant to the transformation above. Inee let g 1 = g g, u 1 = ψ g (u), y 1 = ρ g (y) We have the same system (the system possesses symmetries): t g 1 = f (g 1, u 1 ) y 1 = h(g 1, u 1 )

13 Invariant Extene Kalman filter An EKF writes t ĝ = F(ĝ, u, y). For any g G let ĝ 1 = g ĝ, u 1 = ψ g (u), y 1 = ρ g (y) We want the same formula in the new variables t ĝ1 = F(ĝ 1, u 1, y 1 ) Let L g1 (g) = g 1 g be the left multiplication on G. To be invariant to the transformation the EKF must write 2 t ĝ = f (ĝ, u) + DL ĝ(e) K ( ρĝ 1(y) ρĝ 1( h(ĝ, u) ) ), where the matrix gain K may epen only on Î = ψ ĝ 1(u), E. The observer has the same geometric structure as the system! Bonnabel, Martin, Rouchon. Symmetry-preserving observers. IEEE-TAC.

14 IEKF: How o we tune the gains?? Define the invariant intrinsically efine error η = g 1 ĝ Linearize it for ĝ an g close (η close to e). with C := 1 h(e, Î), δη = (A KC)δη t Aξ := [ ξ, f (e, Î)] 1 f (e, Î) 1ψ(e, Î) ξ an we set Ṗ = AP + PA T + MM T PC T (NN T ) 1 CP, K = PC T (NN T ) 1 Still, how o we choose M, N??

15 Back to the example: IEKF structure t ˆq = ˆq (ω m ˆω b ) + ˆq (K q E) t ˆV = A + 1 ˆq a m ˆq 1 + ˆq (K V E) ˆq 1 â s t ˆω b = K ω E t âs = â s K a E, E = ρˆx 1 (ŷv ŷb ) ρˆx 1 ( yv y B ) = (ˆq 1 ( ˆV ) y V ) ˆq ˆq 1. B ˆq y B The invariant state error g 1 ĝ reas µ q 1 ˆq ν β = q 1 ( ˆV V ) q ˆω b ω b, α â s a s

16 Back to the example: Features of the IEKF Symmetry-preserving structure rotations, translations an scaling in the appropriate frames leave the error system unchange, which is meaningful from an engineering point of view. Soun geometric structure for the quaternion estimation equation: it preserves the unit norm of the estimate quaternion. Larger expecte omain of convergence Proposition: the matrices A an C use for computing the gain matrix K are constant not only in level flight but also on a large set of trajectories (uniform acceleration, rotation with constant angular velocity...). Recall δη = (A KC)δη t

17 Back to the example: Numerical results Experiment: estimate Euler angles. Comparison with a commercial INS-GPS evice MIDG2. ( ) 4 2 Euler angles φ MIDGII estimate φ ( ) 2 2 θ MIDGII estimate θ ( ) ψ MIDGII estimate ψ Time (s)

18 Simulation results: comparison of MEKF an IEKF ( ) real φ Euler angles Gain matrix K(t) for the MEKF 6 φ MEKF 1 φ IEKF ( ) ( ) real θ 1 θ MEKF 5 θ IEKF real ψ ψ MEKF 1 ψ IEKF Time (s) Coefficients Time (s) (m/s) 2 1 Velocity real Vx Vx MEKF Vx IEKF Gain matrix K(t) for the right IEKF (m/s) (m/s) 2 real Vy 1 Vy MEKF Vy IEKF real Vz 5 Vz MEKF Vz IEKF Time (s) Coefficients Time (s)

19 IEKF: what about the noise matrices M, N? The system with noise turne off g = f (g, u) + M(g)w t y = h(g, u) + N(y)v is invariant to the transformation g 1 = g g, u 1 = ψ g (u), y 1 = ρ g (y) It seems logical that the system with noise be invariant as well i.e. t g = f (g 1, u 1 ) + M(g 1 )w y = h(g 1, u 1 ) + N(y 1 )v In particular we take t ĝ = f (ĝ, u) + DL ĝmw. On the example it yiels the same riving noise as for the MEKF.

20 IEKF: what about the noise matrices M, N? With this efinition of the noise matrices, the linearize error equation is a stochastic multiplicative linear ifferential equation t δη = (A KC)δη + Q ( ) ( ) 1 δη, M(e)w) + Q2 δη, KN(e)v), It is NOT the linear moel for which the KF is built δη = (A KC)δη M(e)w + KN(e)v. t Proposition: For both equations, the mean an covariance of the δη are the same up to secon orer terms in the noise amplitue Thus M, N can be chosen on the non-linear system, then buil a Kalman filter with on the linearize system etc.

21 Conclusion When a system possesses symmetries, compell the EKF to preserve them is a way to provie it with the rich geometric structure of the physical system. On a group or a manifol more logical than the usual EKF base on the linear error ˆx x Correspons to a time-invariant linear Kalman filter aroun a whole set of trajectories Particularly suite to aerospace (UAVs) applications, (Symmetries = Galilean invariances).