Symmetry preserving asymptotic observers: theory and examples

Symmetry preserving asymptotic observers:

theory and examples

Pierre Rouchon Coworkers:

Silvère Bonnabel,Philippe Martin,Erwan Salaün

Mines ParisTech Centre Automatique et Systèmes

Mathématiques et Systèmes pierre.rouchon@mines-paristech.fr

2nd Mediterranean Conference on Intelligent Systems and Automation (CISA09)

March 23-25, 2009.

Zarzis, Tunisia

Outline

Asymptotic observers and motivations

Invariant asymptotic observers

Nonlinear observers on Lie group

The linear case:

d

dtx =Ax+Bu y,u known signals y =Cx+Du

Linear observer

A stable filter mixing the input and output signalsu(t)andy(t):

d

dtxˆ=Aˆx +Bu(t)−L(Cxˆ+Du(t)−y(t)) Error system fore:= ˆx−x

d

dte= (A−LC)e

The error system is autonomous (separation principle...)

What about the nonlinear case?¹ d

dtx =f(x,u), y =h(x,u) y,uknown signals Estimator, observer, filter, etc:

d

dtxˆ=f(ˆx,u(t))−L(ˆx,y(t))· h(ˆx,u(t))−y(t)

I Luenberger observer, gain scheduling, high gains, ...

I Extended Kalman Filter (M,N“tuning" matrices) A= ∂f

∂x(ˆx,u) L=−PC^TN C= ∂h

∂x(ˆx,u) d

dtP =AP+PA^T +M⁻¹−PC^TNCP

I Tuning? Domain of convergence? Computational cost?

1See, e.g., G. Besançon (Ed.): Nonlinear Observers and Applications;

Springer; Lecture Notes in Control and Information Sciences , Vol. 363, 2007.J.P. Gauthier, I. Kupka: Deterministic Observation Theory and Applications; Cambridge University Press; 2001.

Redemption by geometry: the nonholonomic car d

dtx =ucosθ d

dty =usinθ d

dtθ=uv h(x,y, θ) = (x,y) u,v =tanϕ/l known A “geometric" observer

d dt



 xˆ yˆ θˆ



=



 ucosθˆ

usinθˆ uv



+





R_−θˆ 0 0 0 0 1



·L·R_θˆ·

ˆx−x yˆ−y

whereLis a 3×2 gain matrix andR_θˆ:=

cosθˆ sinθˆ

−sinθˆ cosθˆ

“Alternative" state error

η_x ηy

=R_θˆ·

xˆ−x yˆ−y

R_θˆ:=

cosθˆ sinθˆ

−sinθˆ cosθˆ

Autonomous error system but for the “free" knownu,v

d dt



 η_x ηy

η_θ



=





u(1−cosη_θ) + (uv +L₃₁η_x +L₃₂η_y)η_y usinηθ−(uv +L₃₁ηx+L₃₂ηy)ηx

0



+L· ηx

ηy

Easy tuning on the linearized error system witha,b,c>0.

d dt



 δη_x δηy

δη_θ



=





uvδη_y uδη_θ−uvδηx

0



+





−|u|a −uv uv −|u|c

0 −ub



 δηx

δηy

Transformation groups and invariance²

LetGbe a Lie Group with identityeandΣbe an open set of R^σ. A(local) transformation group(φ_g)_g∈G onΣis a (smooth) map

(g, ξ)∈G×Σ7→φg(ξ)∈Σ such that:

I φe(ξ) =ξ for allξ

I φg2◦φg1(ξ) =φg2g1(ξ)for allg₁,g₂, ξ The mapξ7→J(ξ)isinvariantif

J(φ_g(ξ)) =J(ξ) for allg, ξ The vector fieldξ7→w(ξ)isinvariantif

w(φ_g(ξ)) =Dφ_g(ξ)·w(ξ) for allg, ξ

2See, e.g., P.J. Olver: Equivalence, Invariants and Symmetry; Cambridge University Press; 1995.

The moving frame method for computing all sorts of invariants³

When dimg ≤dimξ=σ:

we can assumeφg = (φ^a_g, φ^b_g)withφ^a_ginvertible wrtg

I Normalization:

solveφ^a_g(ξ) =cforg ⇒ g=γ(ξ) (ieφ^a_γ(ξ)(ξ) =c)

I the mapξ7→J(ξ) :=φ^b_γ(ξ)(ξ)is invariant

I any other invariant is a function ofJ

I the vector fieldξ7→w(ξ) :=

Dϕ_γ(ξ)(ξ)−1

·Υ,Υconstant vector inR^σ, is invariant

I in particularw_i(ξ) :=

Dϕ_γ(ξ)(ξ)−1

·e_i, i =1, . . . , σ=dimΣ, form an invariant frame

3See P.J. Olver: Classical Invariant Theory; Cambridge University Press;

1999.

Invariant systems and invariant preobservers⁴ d

dtx =f(x,u), x ∈ X ⊂Rⁿ,u∈ U ⊂R^m y =h(x,u), y ∈ Y ⊂R^p

Transformation groups(X,U,Y) = ϕg(x), ψg(u), %g(y) The system_dt^dx =f(x,u)isinvariantif

f ϕ_g(x), ψ_g(u)

=Dϕ_g(x)·f(x,u) for allg,x,u The outputy =h(x,u)isequivariantif

h ϕg(x), ψg(u)

=%g h(x,u)

for allg,x,u The system_dt^dxˆ =F(ˆx,u,y)is apreobserverif

F x,u,h(x,u)

=f(x,u) for allx,u The preobserver _dt^dxˆ=F(ˆx,u,y)isinvariantif

F ϕ_g(ˆx), ψ_g(u), %_g(y)

=Dϕ_g(ˆx)·F(ˆx,u,y) for allg,xˆ,u,y

4Bonnabel, Martin, R: Invariant asymptotic observers. IEEE-TAC, 2008.

Structure of invariant preobservers Every invariant preobserver reads

d

dtxˆ=f(ˆx,u) +W(ˆx)L

I(ˆx,u),E(ˆx,u,y)

E(ˆx,u,y)

I E(ˆx,u,y)invariant output error

I W(ˆx) = w₁(ˆx), ..,w_n(ˆx)

invariant frame

I I(ˆx,u)invariant

I L(I,E)freely chosenn×pgain matrix

Explicit construction by the moving frame method (ˆx,u,y)7→E(ˆx,u,y)∈ Y is aninvariant output errorif

I y 7→E(ˆx,u,y)is invertible for allxˆ,u

I E xˆ,u,h(ˆx,u)

=0 for allxˆ,u

I E ϕg(ˆx), ψg(u), %_g(y)

=E(ˆx,u,y)for allxˆ,u,y

So what?

The error system is “nearly" autonomous:

d

dtη= Υ η,I(ˆx,u)

, whereηis an invariant state error When dimG=dimX, autonomous error system but for the

“free" known invariantI!

(ˆx,x)7→η(ˆx,x)∈ X is aninvariant state errorif

I x 7→η(ˆx,x)is invertible for allxˆand vice versa

I η(x,x) =0 for allx

I η ϕg(ˆx), ϕg(x)

=η(ˆx,x)for allxˆ,x

Benefit of the symmetry-preserving approach:

when dimG=dimX, the observer iseasily tuned for (at least) local convergence around every “permanent" trajectory, with a good local behavior (interesting practical property)

Back to the nonholonomic car withG=SE(2)

Invariance by rotation and translationg = (xg,yg, θg)∈G



 x_g y_g θ_g



·



 x y θ



=





xcosθ_g−ysinθ_g+x_g xsinθ_g+ycosθ_g+y_g

θ+θ_g





ϕ_(xg,yg,θg)(x,y, θ) =



 x_g y_g θ_g



·



 x y θ



, ψ_(xg,yg,θg)(u,v) = u

v

An invariant observer





d dtxˆ

d dtyˆ

d dtθˆ



=



 ucosθˆ

usinθˆ uv



+



 R₋θˆ

0 0

0 0 1



·L·Rθˆ· xˆ−x

yˆ−y

whereLis a 3×2 gain matrix andRθˆ:=

cosθˆ sinθˆ

−sinθˆ cosθˆ

Velocity aided-AHRS : the considered problem⁵

I 3 gyros giveω_m:=ω(body frame)

I 3 acceleros giveam:=a(body frame)

I 3 magnetos gives the Earth magnetic field (body frame)

I An air data system gives thevelocity vector(body frame)

I No biases

"Quaternion" model d

dtq =1 2q∗ω d

dtv =v ×ω

+q⁻¹∗ A ∗q+a

y = yv

yb

=

v q⁻¹∗ B ∗q

“Natural" transformation group ϕ^



q_g v_g





q v

=

q∗q_g q⁻¹_g ∗v ∗qg+vg

ψ^



q_g vg





ω a

=







q_g⁻¹∗ω∗qg

q_g⁻¹∗a∗q_g−...

vg×(q_g⁻¹∗ω∗qg)







ρ^



qg

v_g





y_v y_b

=

q⁻¹_g ∗y_v∗q_g+v_g q_g⁻¹∗y_b∗q_g

5See PhD Thesis of Erwan Salaün where such kinds of observers are extensively developed andtested experimentally on micro-controllers.

A simplified Velocity aided AHRS

I Model withv,aandωmeasured in thebody frame.

Constant gravity and magnetic vectors,AandBin the Earth Frame.

d dtq= 1

2q∗ω d

dtv =v ×ω+q⁻¹∗ A ∗q+a y =

y_v y_b

=

v q⁻¹∗ B ∗q

and(ω,a)are inputs. We want toestimate(q,v).

I In the body Frame: invariance versus rotationqg and velocity translationvg.

Natural transformation groupSE(3) Herex =

q v

,g =

q_g v_g

(G∼SE(3)) and we take:

ϕ^



q_g vg





q v

= q_g

vg

· q

v

=

q∗qg

q⁻¹_g ∗v ∗q_g+v_g

Hereu= a

ω

and we take:

ψ^



q_g vg





a ω

=

q⁻¹_g ∗a∗q_g−v_g×(q⁻¹_g ∗ω∗q_g) q_g⁻¹∗ω∗qg

Herey = y_v

y_b

=

v q⁻¹∗ B ∗q

and we take:

%^



q_g vg





y_v y_b

=

q_g⁻¹∗y_v∗q_g+v_g q_g⁻¹∗y_b∗qg

SE(3)invariance of the system

The set of equations characterizing the system d

dtq= 1

2q∗ω, d

dtv =v×ω+q⁻¹∗ A ∗q+a y =

y_v y_b

=

v q⁻¹∗ B ∗q

is unchanged if, for any q_g

vg

, we set

ϕ^



q_g vg





q v

= Q

V

, ψ^



q_g vg





a ω

= A

Ω

, %^



q_g vg





y_v y_b

= Y_v

Y_b

We have d

dtQ= 1

2Q∗Ω, d

dtV =V ×Ω +Q⁻¹∗ A ∗Q+A Y =

Y_v Y_b

=

V

Q⁻¹∗ B ∗Q

The invariant nonlinear observer

d dtqˆ = 1

2ˆq∗ω(t) +( ¯L^q_vEv + ¯L^q_bE_b)∗ˆq d

dtvˆ = ˆv×ω(t) + ˆq⁻¹∗ A ∗qˆ+a(t) +qˆ⁻¹∗( ¯L^v_vEv + ¯L^v_bE_b)∗qˆ with the following invariant errors:

E_v = ˆq∗(ˆv −y_v(t))∗qˆ⁻¹, E_b=B −ˆq∗y_b(t)∗qˆ⁻¹ andL¯^q_v,L¯^v_v,L¯^q_b andL¯^v_b are constant matrices.

Autonomous errors dynamics: tune the gains to ensure local exponential convergence aroundany system trajectory with uniform Lyapunov exponents.⁶

6See Bonnabel, Martin, R.: Invariant asymptotic observers;IEEE-TAC, 2008. See also Mahony, Hamel, Pflimlin: Nonlinear Complementary Filters on the Special Orthogonal Group, IEEE-TAC 2008.

Autonomous errors dynamics

Consider the invariant state-estimation errors ηq= ˆq∗q⁻¹, ηv =q∗(ˆv −v)∗q⁻¹.

Their dynamics satisfy the followingautonomousequation:

d

dtη_q=L¯^q_v(η_q∗η_v ∗η_q⁻¹) + ¯L^q_b(B −η_q∗ B ∗η⁻¹_q )

∗η_q d

dtη_v =η_q⁻¹∗

A+ ¯L^v_v(η_q∗η_v∗η_q⁻¹) + ¯L^v_b(B −η_q∗ B ∗η⁻¹_q )

∗η_q− A

Convergence of the linearized error system

Indeed forqˆ andˆv close toqandv we have δE_v =δηv

and

δE_b=−δη_q∗ B+B ∗δηq =2B ×δηq

Thelinearized errorequation writes:

d

dtδηq= ¯L^q_vδηv+2L¯^q_b(B ×δηq) d

dtδηv =2A ×δηq+ ¯L^v_vδηv +2L¯^v_b(B ×δηq)

Let us choose L¯^q_v =





0 −M₁₂ 0 M₂₁ 0 0

0 0 0



 L¯^v_v =−





N₁₁ 0 0 0 N₂₂ 0

0 0 N₃₃





L¯^q_b=





0 0 0

0 0 0

−λB² λB¹ 0



 L¯^v_b =





0 0 0 0 0 0 0 0 0





In (Earth-fixed) coordinates,

δη_q:=







 0 δη¹_q δη²_q δη³_q









, δη_v :=



 δη_v¹ δη_v² δη_v³



 and A=



 0 0 A³



,

The error system breaks in four decoupled subsystems:

I thelongitudinalsubsystem d

dt δη_q²

δη_v¹

=

0 M₂₁

−2A³ −N₁₁

δη_q² δη_v¹

I thelateralsubsystem d

dt δη_q¹

δη_v²

=

0 −M₁₂ 2A³ −N₂₂

δη_q¹ δη_v²

I theverticalsubsystem d

dtδη³_v =−N₃₃δη_v³

I theheadingsubsystem d

dtδη_q³=λB³(B¹δη_q¹− B²δη²_q)−λ((B¹)²+ (B²)²)δη³_q

Invariant dynamics and observers on a Lie group⁷ Consider aleft-invariant dynamicson a Lie group G

d

dtg =DL_gωs(t) y =h(g)

withhanG-équivariantoutput : for allg₁∈Gthere existsρg₁

such that for allg₂∈G:

h(g₁g₂) =ρg₁(h(g₂))

withρg1◦ρg2 =ρg1g2. Any invariant observer writes d

dtgˆ =DL_ˆgω_s(t) +DL_ˆg

n

X

i=1

L_i(ρ_ˆg−1(y))W_i

!

avec(W₁, ..W_n)basis of the Lie algebraL_i(h(e))) =0

7Bonnabel, Martin, R.: Non-linear Symmetry preserving observers on Lie groups. IEEE-TAC, June 2009.

The invariant state error is

η =g⁻¹gˆ The error equation

d

dtη =DL_ηωs−DR_ηωs+DL_η

n

X

i=1

L_i◦h(η⁻¹)W_i reminds

d

dte= (A(t) +LC)e

I What if the condition on the output ish(g₁g₂)=ρg₂(h(g₁))?

I η = ˆgg⁻¹⇒AUTONOMOUS error equation !⁸

I reminds _dt^de=LCe

8This autonomous character resulting form left translations on the state and right translations on the output was first noticed in Bonnabel, Martin, R.:

Groupe de Lie et observateur non-linéaire; CIFA 2006 (Conférence

Internationale Francophone d’Automatique), Bordeaux, France, 2006. It has been also noticed in 2008 and independently by Mahony and co-workers:

http://arxiv.org/abs/0810.0748 and http://arxiv.org/abs/0805.0828

Conclusion

Invariance is just a way to"put physics"in nonlinear estimation and filtering processes.

I Chemical reactorswhere the group is just associated to changes of units and invariance relies on the fact that material and energy balance equations do not depend on chosen units.

I Quantum systemsdescribed by Lindblad-Kossakovski differential equation

d

dtρ= −ı

~ [H, ρ] +2LρL^†−L^†Lρ−ρL^†L, y =tr LρL^† whereρis the density operator (symmetric, semi-positive with tr(ρ) =1),HandLare operators. All these operators are defined up to a change of framesU∈G=U(n)via:

(ρ,H,L)7→(UρU^†,UHU^†,ULU^†)

I Infinite dimensional systems described byPDE’ssuch as the Saint-Venant equationhttp://arxiv.org/abs/0809.1400.

I For inertial navigation anddata fusionbetween camera, acceleros, gyros and magnetos... Galilean invariance could certainly be exploited in a much more systematic way....