Equivariant Filter Design for Discrete-time Systems

Yixiao Ge
Systems Theory and Robotics Group
Australian Centre for Robotic Vision
Australian National University
ACT, 2601, Australia
[email protected]
&

Pieter van Goor
Systems Theory and Robotics Group
Australian Centre for Robotic Vision
Australian National University
ACT, 2601, Australia
[email protected]
&

Robert Mahony
Systems Theory and Robotics Group
Australian Centre for Robotic Vision
Australian National University
ACT, 2601, Australia
[email protected]

Abstract

The kinematics of many nonlinear control systems, especially in the robotics field, admit a transitive Lie-group symmetry, which is useful in high performance observer design. The recently proposed equivariant filter (EqF) exploits equivariance to generate high performance filters for a wide range of real-world systems. However, existing work on the equivariant filter, and equivariance of control systems in general, is based on a continuous-time formulation. In this paper, we first present the equivariant structure of a discrete-time system. We then use this to propose a discrete-time version of the equivariant filter. A novelty of the approach is that the geometry of the symmetry group naturally appears as parallel transport in the reset step of the filter. Preliminary results for linear second order kinematics with separate bearing and range measurements indicate that the discrete EqF significantly outperforms both a discretized version of the continuous EqF and a classical discrete EKF.

1 INTRODUCTION

Many nonlinear control systems are naturally posed on homogeneous spaces, that is manifolds that submit to a transitive group action by a Lie group. Such systems carry the property of equivariance, that is, the defining dynamics can be mapped throughout state-space by the group action. The geometry and symmetry of those systems have been recognized since the seventies [1], [2], although this did not translate into observer and filter design until the 1990s. Salcudean proposed a non-linear observer for the attitude estimation of a satellite [3]. Thienel and Sanner extended the original observer and incorporated bias estimation [4]. Parallel work from Aghannan and Rouchon introduced a state observer design for Lagrangian systems using the invariance property [5]. Later around 2005, with the emerging unmanned aerial vehicle industry, there was a burst of work to develop a simple, robust observer design for attitude control. Mahony et al. [6] proposed an asymptotically stable complementary filter for attitude estimation posed directly on the matrix Lie group $\mathbf{SO}(3)$ , which has an almost-global convergence property. In parallel, Bonnabel et al. proposed a general theory for the Invariant Extended Kalman Filter (IEKF) in a series of works [7], [8]. They provided a design methodology for systems on Lie groups with invariance properties, which was later extended to the class of ‘group affine’ systems [9]. These works fully exploited the underlying symmetry properties of the system and had great impact in the control and robotics community. Recently, van Goor et al. [10] and Mahony et al. [11] proposed the Equivariant Filter (EqF), a general filter design for systems on homogeneous space.

Less attention has been focused on the observer problem for discrete-time systems. There are two common approaches to design a discrete filter for a continuous system with discrete-time measurements. The first is to design a continuous-time filter and then discretize the filter, while the second approach is to discretize the continuous system and design a discrete-time filter for the discrete system [12]. One of the advantages of the second is a range of existing geometric integrators to discretize systems on manifolds, provided by Crouch et al. and Hairer et al. [13][14]. Nonetheless, both approaches introduce approximation error into the system with the discretization process, although, the second is known to have better stability properties [15]. Preliminary work on stochastic filtering of continuous- and discrete-time systems was carried out in the late 1970s and early 1980s, and is summarised in Maybeck’s book [16]. More recently, the question of discrete filtering for systems on Lie-groups has received considerable attention. Bourmaud et al. generalized the extended Kalman filter to matrix Lie groups for discrete-time systems [17]. Barrau et al. proposed the IEKF for discrete-time dynamical systems on Lie groups, which has been implemented for navigation problems [18].

In this paper, we propose an observer design methodology for a class of discrete-time systems on homogeneous spaces using the equivariance property. This work is parallel to recent work on Equivariant filter (EqF) design [10], that has yielded filters that are obtained as a discretization of a continuous-time filter design. We assume that a discretization of the continuous-time system is provided as the model, exploiting for example the literature on discretization of systems on Lie-groups [13][14][19][20]. In particular, these discretizations yield updates in the form of a symmetry transformation that approximates the integration of the continuous-time system for constant input acting on the present state. The symmetry structure is exploited in the filter development and we believe lies at the heart of the performance gains reported.

One of our key contributions is that we propose a novel approach to model filter reset as the parallel transport of covariance. In a conventional extended Kalman filter, the update step provides an estimate of the new mean and covariance based on the predicted state and new measurements. We implement Bayesian fusion in local coordinates centred at the predicted state estimate generating both a new state estimate and covariance expressed in the local coordinates, and then, drawing from recent insight [21], parallel transport the covariance to new coordinates centred at the updated state estimate. Our approach provides an alternative, and we claim more general, perspective on work done by Markley in [22] twenty years ago and generalised recently by Mueller et al. [23]. We apply the proposed filter to an example of second-order kinematics with separate range and bearing measurements and run the comparison with a discretized version of a continuous-time EqF and conventional discrete extended Kalman filter. The simulations demonstrate the advantage of both EqF filters versus the EKF due to proper modelling of the reset step during the transient response, and significant performance gains of the discrete EqF compared to both comparison filters in the asymptotic response. We believe this performance gain is due to the fact that the error incurred in a discrete state update implemented as a symmetry transform is better adapted to the discrete equivariant filter equations than the error incurred by the discretization of a continuous-time filter, even when the associated filter was based on equivariant design principles in continuous-time.

2 PRELIMINARIES

Let $\mathcal{M}$ be a smooth manifold. The tangent space at a point $\xi\in\mathcal{M}$ is denoted $\mathrm{T}_{\xi}\mathcal{M}$ . A diffeomorphism on $\mathcal{M}$ is a smooth map $F:\mathcal{M}\rightarrow\mathcal{M}$ .

Let $\mathfrak{X}(\mathcal{M})$ denote the space of vector fields over $\mathcal{M}$ . Let $\mathcal{D}(\mathcal{M})$ denote the group (under function concatenation) of diffeomorphisms of $\mathcal{M}$ . Let $f\in\mathfrak{X}(\mathcal{M})$ then the flow $\mathcal{F}:\mathbb{R}\times\mathcal{M}\rightarrow\mathcal{M}$ of $f$ is defined as the family of diffeomorphisms $\mathcal{F}(t,\cdot)\in\mathcal{D}(\mathcal{M})$ , $\mathcal{F}(t,\xi_{0})=\xi(t)$ , where $\xi(t)$ is the solution of $\dot{\xi}=f(\xi)$ for $\xi(0)=\xi_{0}$ .

Given a differentiable function between smooth manifolds $h:\mathcal{M}\rightarrow\mathcal{N}$ , its derivative at $\xi_{\circ}$ is written as

\displaystyle\mathrm{D}_{\xi|\xi_{\circ}}h(\xi):\mathrm{T}_{\xi_{\circ}}\mathcal{M}\rightarrow\mathrm{T}_{\xi_{\circ}}\mathcal{N}.

The notation $\mathrm{D}h(\xi):\mathrm{T}\mathcal{M}\rightarrow\mathrm{T}\mathcal{N}$ denotes the differential of $h$ with an implicit base point.

Let $\mathbf{G}$ be a general Lie group with Lie algebra $\mathfrak{g}$ , $\mathrm{id}$ denotes the identity element of $\mathbf{G}$ . For arbitrary $X,Y\in\mathbf{G}$ , the left and right translations are denoted $\mathrm{L}_{X}$ and $\mathrm{R}_{X}$ , and are defined by

\mathrm{L}_{X}(Y):=XY,\quad\mathrm{R}_{X}(Y):=YX.

The Lie algebra $\mathfrak{g}$ is isomorphic to a vector space $\mathbb{R}^{n}$ with the same dimension. We use the wedge $(\cdot)^{\wedge}:\mathbb{R}^{n}\rightarrow\mathfrak{g}$ and vee $(\cdot)^{\vee}:\mathfrak{g}\rightarrow\mathbb{R}^{n}$ operators to map between the Lie algebra and vector space. The adjoint map for $\mathbf{G}$ , $\operatorname{Ad}_{X}:\mathfrak{g}\rightarrow\mathfrak{g}$ is defined by

\displaystyle\operatorname{Ad}_{X}U=\mathrm{D}\mathrm{L}_{X}\mathrm{D}\mathrm{R}_{X^{-1}}U

for any $X\in\mathbf{G}$ and $U\in\mathfrak{g}$ . When $\mathbf{G}$ is a matrix Lie group, we can define the adjoint matrix $\operatorname{Ad}^{\vee}_{X}:\mathbb{R}^{n}\rightarrow\mathbb{R}^{n}$ as

\displaystyle\operatorname{Ad}^{\vee}_{X}U=(\operatorname{Ad}_{X}U)^{\vee}

For $X\in\mathbf{G}$ the inner automorphism $\mathrm{I}_{X}:\mathbf{G}\rightarrow\mathbf{G}$ of $\mathbf{G}$ is the smooth map

\mathrm{I}_{X}(Z):=XZX^{-1}=\mathrm{L}_{X}\mathrm{R}_{X^{-1}}Z,\quad Z\in\mathbf{G}.

A right group action of a Lie group $\mathbf{G}$ on a manifold $\mathcal{M}$ is a smooth map $\phi:\mathbf{G}\times\mathcal{M}\rightarrow\mathcal{M}$ that satisfies

\displaystyle\phi(X,\phi(Y,\xi))=\phi(YX,\xi)\quad\textrm{and}\quad\phi(\mathrm{id},\xi)=\xi

for any $X,Y\in\mathbf{G}$ and $\xi\in\mathcal{M}$ . It induces the partial maps $\phi_{X}:\mathcal{M}\rightarrow\mathcal{M}$ and $\phi_{\xi}:\mathbf{G}\rightarrow\mathcal{M}$ which are defined by $\phi_{X}(\xi):=\phi(X,\xi)=:\phi_{\xi}(X)$ respectively.

Proposition 2.1.

Any right action $\phi:\mathbf{G}\times\mathcal{M}\rightarrow\mathcal{M}$ induces a right action on the group of diffeomorphisms of $\mathcal{M}$ , denoted $\Phi:\mathbf{G}\times\mathcal{D}(\mathcal{M})\rightarrow\mathcal{D}(\mathcal{M})$ , and defined by

\Phi(X,F)=\phi_{X}\circ F\circ\phi_{X}^{-1},

(1)

for any $F\in\mathcal{D}(\mathcal{M})$ .

Proof.

Let $X,Y\in\mathbf{G}$ and $F\in\mathcal{D}(\mathcal{M})$ . To prove compatibility, compute

	$\displaystyle\Phi_{Y}(\Phi_{X}(F))$	$\displaystyle=\Phi_{Y}(\phi_{X}F\phi_{X}^{-1})$
		$\displaystyle=\phi_{Y}(\phi_{X}F\phi_{X}^{-1})\phi_{Y}^{-1}$
		$\displaystyle=\phi_{XY}F\phi_{(XY)^{-1}}=\Phi_{XY}F.$

To prove the identity,

\displaystyle\Phi_{\mathrm{id}}F=\phi_{\mathrm{id}}F\phi_{\mathrm{id}}=F,

as required. ∎

3 PROBLEM FORMULATION

3.1 Discrete-time System

Let $\mathcal{M}$ be a smooth manifold termed the state space, $\mathbb{V}$ be a vector space termed the input space. Let $F:\mathcal{M}\times\mathbb{V}\to\mathcal{M}$ be a family of diffeomorphisms on $\mathcal{M}$ , and $F(\cdot,u_{k})\in\mathcal{D}(\mathcal{M})$ for all $u_{k}\in\mathbb{V}$ . The class of discrete-time systems on $\mathcal{M}$ that we consider can be written as

\xi_{k+1}=F_{u_{k+1}}(\xi_{k})=F(\xi_{k},u_{k+1}),\quad\xi_{0}\in\mathcal{M},

(2)

The configuration output $y_{k}=h(\xi_{k})$ is given by a function $h:\mathcal{M}\rightarrow\mathcal{N}\subset\mathbb{R}^{n}$ , where $\mathcal{N}$ is a smooth manifold termed the output space embedded in $\mathbb{R}^{n}$ . In practice, there will be disturbances on both the update and the measurements process. The noise process for the measurement is a Gaussian modelled in the embedding space $\mathbb{R}^{n}$ , while for the process noise, we model a Gaussian disturbance in the local exponential coordinates during the update step (the $P$ matrix in (29)).

3.2 State Symmetry

Let $\mathbf{G}$ be a Lie group with Lie algebra $\mathfrak{g}$ , and $\mathcal{M}$ be a homogeneous space of $\mathbf{G}$ , then there exists a smooth, transitive, right group action of $\mathbf{G}$ on $\mathcal{M}$ ,

\phi:\mathbf{G}\times\mathcal{M}\rightarrow\mathcal{M}.

(3)

A discrete lift for the system (2) is a map $\Lambda:\mathcal{M}\times\mathbb{V}\rightarrow\mathbf{G}$ with the lift condition

\phi_{\xi_{k}}(\Lambda(\xi_{k},u_{k+1}))=F_{u_{k+1}}(\xi_{k}),

(4)

for every $\xi\in\mathcal{M}$ and $u\in\mathbb{V}$ . The lift $\Lambda:\mathcal{M}\times\mathbb{V}\rightarrow\mathbf{G}$ is used to define a lifted system on the symmetry Lie group $\mathbf{G}$ . First choose an arbitrary origin $\xi^{\circ}\in\mathcal{M}$ , that acts as the origin in a global coordinate parametrization $\phi_{\xi^{\circ}}:\mathbf{G}\rightarrow\mathcal{M}$ . The lifted system is written

X_{k+1}=X_{k}\Lambda(\phi_{\xi^{\circ}}(X_{k}),u_{k+1}),\quad X(0)\in\mathbf{G}.

(5)

Lemma 3.1.

Consider the lifted system (5). If the initial condition $X(0)\in\mathbf{G}$ satisfies

\phi_{\xi^{\circ}}(X(0))=\xi(0),

then the trajectory of the lifted system $X_{k}$ projects down to the original kinematic system with the map $\phi_{\xi^{\circ}}:\mathbf{G}\rightarrow\mathcal{M}$

\phi_{\xi^{\circ}}(X)\equiv\xi,

(6)

for all time.

Proof.

Let $X_{k}$ be a solution of (5) and let $\xi_{k}=\phi_{\xi^{\circ}}(X_{k})$ . One has

	$\displaystyle\xi_{k+1}=\phi_{\xi^{\circ}}(X_{k+1})$	$\displaystyle=\phi_{\xi^{\circ}}(X_{k}\Lambda(\phi_{X_{k}}(\xi^{\circ}),u_{k+1}))$
		$\displaystyle=\phi(\Lambda(\xi_{k},u_{k+1}),\phi(X_{k},\xi^{\circ}))$
		$\displaystyle=\phi_{\xi_{k}}(\Lambda(\xi_{k},u_{k+1}))$
		$\displaystyle=F(\xi_{k},u_{k+1}),$

which satisfies (2). ∎

4 EQUIVARIANT SYSTEMS

4.1 Equivariance

A discrete-time system (2) is termed equivariant if there exists a group action

\displaystyle\psi:\mathbf{G}\times\mathbb{V}\rightarrow\mathbb{V}

(7)

which satisfies

\phi_{X}(F(\xi,u))=F(\phi_{X}(\xi),\psi_{X}(u)),

(8)

for any $X\in\mathbf{G}$ , $\xi\in\mathcal{M}$ and $u\in\mathbb{V}$ . Consider a discrete-time equivariant system with the group action, then

$\displaystyle F_{\psi_{X}(u)}(\xi)$	$\displaystyle=F_{\psi_{X}(u)}(\phi_{X}(\phi_{X}^{-1}(\xi)))$
	$\displaystyle=\phi_{X}(F_{u}(\phi_{X}^{-1}(\xi))$
	$\displaystyle=\Phi_{X}F_{u}(\xi),$	(9)

where $\Phi$ is the group action on $\mathcal{D}(\mathcal{M})$ (1). We can also conclude from (9) that if $\psi$ exists, i.e. the system (2) is equivariant, then $\psi$ is implicitly determined by the induced group action $\Phi$ .

4.2 Equivariant Lift

For a system with state symmetry $\phi:\mathbf{G}\times\mathcal{M}\rightarrow\mathcal{M}$ and input symmetry $\psi:\mathbf{G}\times\mathbb{V}\rightarrow\mathbb{V}$ , the discrete lift is equivariant if

\Lambda(\phi(X,\xi),\psi(X,u))=X^{-1}\Lambda(\xi,u)X=\mathrm{I}_{X^{-1}}\Lambda(\xi,u),

(10)

for any $X\in\mathbf{G}$ , $\xi\in\mathcal{M}$ and $u\in\mathbb{V}$ .

5 OBSERVER ARCHITECTURE

In this section we provide the formulation of an equivariant observer for a discrete-time system. The key in the formulation is the equivariant error which measures the global error between the manifold and the symmetry group.

5.1 Observer Formulation

Let $\mathcal{M}$ be a smooth manifold. Consider a kinematic model with state symmetry $\phi:\mathbf{G}\times\mathcal{M}\rightarrow\mathcal{M}$ , input symmetry $\psi:\mathbf{G}\times\mathbb{V}\rightarrow\mathbb{V}$ and trajectory denoted by $\xi_{k}$ . Choose an arbitrary but fixed origin $\xi^{\circ}\in\mathcal{M}$ . The lifted system is defined by the evolution function

{X}_{k+1}={X}_{k}\Lambda(\phi_{\xi^{\circ}}({X}_{k}),u_{k+1}),\quad\phi(X(0),\xi^{\circ})=\xi(0)

(11)

where $X(0)\in\mathbf{G}$ .

Define the observer $\hat{X}_{k}$ dynamics on the symmetry Lie group by

\hat{X}_{k+1}=\exp(\Delta_{k+1})\hat{X}_{k}\Lambda(\phi_{\xi^{\circ}}(\hat{X}_{k}),u_{k+1}),\quad\hat{X}(0)=\mathrm{id}

(12)

where $\Delta_{k+1}\in\mathfrak{g}$ is the time varying correction term that remains to be chosen. The corresponding state estimate of the observer is given by

\hat{\xi}_{k}=\phi_{\xi^{\circ}}(\hat{X}_{k}).

The equivariant error is defined as

e_{k}=\phi_{\hat{X}_{k}^{-1}}(\xi_{k}).

(13)

This error is a key concept in equivariant filter design, since it measures the difference between the observer trajectory $\hat{X}_{k}\in\mathbf{G}$ and the system trajectory $\xi_{k}\in\mathcal{M}$ which live in two different spaces. Note that if $\hat{\xi_{k}}=\xi_{k}$ , i.e the observer estimate is correct, then the error $e_{k}=\xi^{\circ}$ .

Define the origin input

\displaystyle u^{\circ}_{k+1}:=\psi(\hat{X}_{k}^{-1},u_{k+1}),

(14)

which is measurable because both $\hat{X}$ and $u$ are known.

5.2 Error Dynamics

The EqF is derived by linearising the dynamics of the equivariant error $e_{k}$ .

Lemma 5.1.

Consider an equivariant system with lifted model (5) and the observer (12), the error dynamics are given by

e_{k+1}=\phi(\Lambda(e_{k},u^{\circ}_{k+1})\Lambda(\xi^{\circ},u^{\circ}_{k+1})^{-1}\exp(-\Delta_{k+1}),e_{k}),

(15)

and only depend on the origin input ${u}^{\circ}_{k+1}=\psi(\hat{X}_{k}^{-1},u_{k+1})$ and the error $e_{k}$ itself.

Proof.

Let $X_{k}$ be a solution to the lifted model (5) satisfying $\phi(X(0),\xi^{\circ})=\xi(0)$ . Define the group error $E_{k}:=X_{k}\hat{X}_{k}^{-1}$ , compute the dynamics of $E$ :

	$\displaystyle E_{k+1}$	$\displaystyle=X_{k}\Lambda(\phi_{\xi^{\circ}}(X_{k}),u_{k+1})\Lambda(\phi_{\xi^{\circ}}(\hat{X}_{k}),u_{k+1})^{-1}\hat{X}_{k}^{-1}\exp(-\Delta_{k+1}),$
		$\displaystyle=E_{k}\hat{X}_{k}\Lambda(\phi_{\xi^{\circ}}(E_{k}\hat{X}_{k}),u_{k+1})\hat{X}_{k}^{-1}\hat{X}_{k}\Lambda(\phi_{\xi^{\circ}}(\hat{X}_{k}),u_{k+1})^{-1}$
		$\displaystyle\qquad\qquad\qquad\qquad\qquad\qquad\qquad\hat{X}_{k}^{-1}\exp(-\Delta_{k+1}),$
		$\displaystyle=E_{k}\Lambda(\phi_{\xi^{\circ}}(E_{k}),\psi(\hat{X}_{k}^{-1},u_{k+1}))\Lambda(\phi_{\xi^{\circ}}(I),\psi(\hat{X}_{k}^{-1},u_{k+1}))^{-1}$
		$\displaystyle\qquad\qquad\qquad\qquad\qquad\qquad\qquad\exp(-\Delta_{k+1}),$
		$\displaystyle=E_{k}\Lambda(\phi(E_{k},\xi^{\circ}),u^{\circ}_{k+1})\Lambda(\xi^{\circ},u^{\circ}_{k+1})^{-1}\exp(-\Delta_{k+1}),$
		$\displaystyle=E_{k}\Lambda(e_{k},u^{\circ}_{k+1})\Lambda(\xi^{\circ},u^{\circ}_{k+1})^{-1}\exp(-\Delta_{k+1}).$

Note that

\displaystyle e_{k}:=\phi(\hat{X_{k}}^{-1},\xi_{k})=\phi(X_{k}\hat{X_{k}}^{-1},\xi^{\circ})=\phi(E_{k},\xi^{\circ}).

Substitute the dynamics of $E_{k}$ into this relationship and one obtains

	$\displaystyle e_{k+1}$	$\displaystyle=\phi(E_{k+1},\xi^{\circ}),$
		$\displaystyle=\phi(E_{k}\Lambda(e_{k},u^{\circ}_{k+1})\Lambda(\xi^{\circ},u^{\circ}_{k+1})^{-1}\exp(-\Delta_{k+1}),\xi^{\circ}),$
		$\displaystyle=\phi(\Lambda(e_{k},u^{\circ}_{k+1})\Lambda(\xi^{\circ},u^{\circ}_{k+1})^{-1}\exp(-\Delta_{k+1}),\phi(E_{k},\xi^{\circ})),$
		$\displaystyle=\phi_{e_{k}}(\Lambda(e_{k},u^{\circ}_{k+1})\Lambda(\xi^{\circ},u^{\circ}_{k+1})^{-1}\exp(-\Delta_{k+1})),$

as required. ∎

5.3 Linearisation

To linearise the error $e_{k}\in\mathcal{M}$ requires a chart of local coordinates for the state. Fix a local coordinate chart $\varTheta:\mathcal{M}\rightarrow\mathbb{R}^{m}$ around the origin ${\xi}^{\circ}$ and assume that $e$ remains in a neighborhood of the origin for all time. We use the notation $\varTheta$ and $\varepsilon$ to represent the chart and the local coordinates of the error respectively,

\displaystyle\varepsilon_{k}=\varTheta(e_{k}).

(16)

Proposition 5.2.

Let $\varTheta$ be a local coordinate chart on $\mathcal{M}$ in an open neighborhood around ${\xi}^{\circ}$ . The linearised dynamics of $e_{k}$ about $\varepsilon_{k}=0$ and $\Delta_{k}=0$ are

$\displaystyle\varepsilon_{k+1}$	$\displaystyle\approx A_{k+1}\varepsilon_{k}+O(\lvert\varepsilon\rvert^{2}),$	(17)
$\displaystyle A_{k+1}$	$\displaystyle=\mathrm{D}\varTheta\cdot\mathrm{D}\phi_{\xi^{\circ}}(\mathrm{id})\cdot\mathrm{D}\mathrm{R}_{\Lambda({\xi}^{\circ},u^{\circ}_{k+1})^{-1}}(\Lambda({\xi}^{\circ},{u}^{\circ}_{k+1}))$
	$\displaystyle\qquad\quad\cdot\mathrm{D}_{\xi^{\circ}}\Lambda(\xi^{\circ},{u}^{\circ}_{k+1})\cdot\mathrm{D}\varTheta^{-1}.$	(18)

Proof.

Define the error drift term

\displaystyle\tilde{\Lambda}_{\xi^{\circ}}(e_{k},u^{\circ}_{k+1})=\Lambda(e_{k},u^{\circ}_{k+1})\Lambda(\xi^{\circ},u^{\circ}_{k+1})^{-1}.

The global state error $e_{k}$ has the dynamics

\displaystyle e_{k+1}

\displaystyle=\phi_{e_{k}}(\tilde{\Lambda}_{\xi^{\circ}}(e_{k},u^{\circ}_{k+1})\exp(-\Delta_{k+1})).

(19)

Then,

\varepsilon_{k+1}=\varTheta(\phi_{\varepsilon^{-1}(\varepsilon_{k})}(\tilde{\Lambda}_{\xi^{\circ}}(\varepsilon^{-1}(\varepsilon_{k}),u^{\circ}_{k+1})\exp(-\Delta_{k+1}))).

(20)

Linearising the above equation (20) about $\varepsilon_{k}=0$ and $\Delta_{k}=0$ yields

	$\displaystyle\varTheta($	$\displaystyle\phi_{\varTheta^{-1}(\varepsilon_{k})}(\tilde{\Lambda}_{\xi^{\circ}}(\varTheta^{-1}(\varepsilon_{k}),u^{\circ}_{k+1})))$
		$\displaystyle\approx\varTheta(\phi_{\varTheta^{-1}(\varepsilon_{k})}(\tilde{\Lambda}_{\xi^{\circ}}(\varTheta^{-1}(0),u^{\circ}_{k+1})))$
		$\displaystyle+\mathrm{D}\varTheta\cdot\mathrm{D}\phi_{\xi^{\circ}}(\mathrm{id})\cdot\mathrm{D}\tilde{\Lambda}(\xi^{\circ},u^{\circ}_{k+1})\cdot\mathrm{D}\varTheta^{-1}(\varepsilon_{k})+O(\lvert\varepsilon\rvert^{2})$
		$\displaystyle=A_{k+1}\varepsilon_{k}+O(\lvert\varepsilon\rvert^{2}),$
	$\displaystyle A_{k+1}$	$\displaystyle=\mathrm{D}\varTheta\cdot\mathrm{D}\phi_{\xi^{\circ}}(\mathrm{id})\cdot\mathrm{D}\mathrm{R}_{\Lambda({\xi}^{\circ},u^{\circ}_{k+1})^{-1}}(\Lambda({\xi}^{\circ},{u}^{\circ}_{k+1}))$
		$\displaystyle\qquad\quad\qquad\qquad\cdot\mathrm{D}_{\xi^{\circ}}\Lambda(\xi^{\circ},{u}^{\circ}_{k+1})\cdot\mathrm{D}\varTheta^{-1},$

as required. ∎

Now consider the global state error $e_{k}$ and the true system state $\xi_{k}$ , the system output $h(\xi)\in\mathcal{N}\subset\mathbb{R}^{n}$ can be written as

\displaystyle h(\xi_{k})=h(\phi(\hat{X}_{k},e_{k}))=h(\phi_{\hat{X}_{k}}(\varTheta^{-1}(\varepsilon_{k}))).

(21)

Define the output residual $\tilde{y}_{k}\in\mathbb{R}^{n}$ to be $\tilde{y}_{k}=h(\xi_{k})-h(\hat{\xi}_{k})$ . Linearise $\tilde{y}_{k}$ at $\varepsilon_{k}=0$ ,

$\displaystyle\tilde{y}_{k}$	$\displaystyle=y_{k}-\hat{y}_{k},$
	$\displaystyle=h(\phi_{\hat{X}_{k}}(\varTheta^{-1}(0)))+\mathrm{D}h(\hat{\xi}_{k})\cdot\mathrm{D}\phi_{\hat{X}_{k}}(\xi^{\circ})$
	$\displaystyle\qquad\qquad\qquad\qquad\cdot\mathrm{D}\varTheta^{-1}(\varepsilon_{k})+O(\lvert\varepsilon_{k}\rvert^{2})-\hat{y}_{k},$
	$\displaystyle=\mathrm{D}h(\hat{\xi}_{k})\cdot\mathrm{D}\phi_{\hat{X}_{k}}(\xi^{\circ})\cdot\mathrm{D}\varTheta^{-1}(\varepsilon_{k})+O(\lvert\varepsilon_{k}\rvert^{2}),$
	$\displaystyle=C_{k}\varepsilon_{k}+O(\lvert\varepsilon_{k}\rvert^{2}),$
$\displaystyle C_{k}$	$\displaystyle=\mathrm{D}h(\hat{\xi}_{k})\cdot\mathrm{D}\phi_{\hat{X}_{k}}(\xi^{\circ})\cdot\mathrm{D}\varTheta^{-1}.$	(22)

6 OBSERVER DESIGN

In this section, we use the concept of concentrated Gaussian distributions and pose the distributions with local coordinates. Then we derive the EqF equations for predict, update and reset steps separately, which is based on the extended Kalman Filter applied to the equivariant error.

6.1 Concentrated Gaussians on homogeneous spaces

We extend the concept of concentrated Gaussian distribution on Lie groups [24],[17] to homogeneous spaces and introduce the extended concentrated Gaussian distribution, which is a generalization of the normal distribution on the Euclidean space.

Let $\xi\in\mathcal{M}$ be a random variable on the manifold. Fix an arbitrary origin on the manifold $\xi^{\circ}\in\mathcal{M}$ . Fix $\mathfrak{h}$ to be a horizontal subspace of $\mathfrak{g}$ . Define a local map $\phi_{\xi^{\circ}}\cdot\exp_{\mathfrak{h}}:\mathfrak{h}\rightarrow\mathcal{M}$ . Since $\mathfrak{h}$ is horizontal, this map is always locally well defined and injective. Now define a local coordinate chart $\varTheta_{\mathfrak{h}}:\mathcal{M}\rightarrow\mathfrak{h}$ to be

\displaystyle\varTheta_{\mathfrak{h}}:=(\phi_{\xi^{\circ}}\cdot\exp_{\mathfrak{h}})^{-1},

(23)

on a neighborhood of $\xi^{\circ}\in\mathcal{M}$ such that $\phi_{\xi^{\circ}}\cdot\exp$ is bijective. If the homogeneous space is reductive, then there is a natural choice of $\mathfrak{h}$ [25] which is the normal coordinate, however, the construction is always possible.

Given $X\in\mathbf{G}$ is an element on the symmetry group, define the following probability function:

\displaystyle p(\xi)=\alpha e^{-\frac{1}{2}(\varTheta_{\mathfrak{h}}(\phi(X^{-1},\xi))^{\top}\Sigma^{-1}\varTheta_{\mathfrak{h}}(\phi(X^{-1},\xi)))},

(24)

where $\alpha$ is a normalizing factor and $\Sigma$ is a positive-definite matrix. Define $\epsilon$ as

\displaystyle\epsilon=\varTheta_{\mathfrak{h}}(\phi(X^{-1},\xi))

(25)

where the assumption is made that $\phi(X^{-1},\xi)$ remains in the domain of definition of the local coordinate chart. The concentrated Gaussian distribution is obtained by assigning $\epsilon\sim\mathbf{N}(0,\Sigma)$ to a Gaussian distribution. We denote distribution induced on $\xi$ via (25) to be the concentrated Gaussian distribution and denote this by $\xi\sim\mathbf{N}_{\phi}(X,0,\Sigma)$ . By construction, the expected value of $\xi$ is $\mathrm{E}[\xi]=\phi(X,\xi^{\circ})$ . Thus, for $\xi\sim\mathbf{N}_{\phi}(X,0,\Sigma)$ one has $\xi=\phi_{X}(\varTheta_{\mathfrak{h}}^{-1}(\epsilon))$ , $\epsilon\sim\mathbf{N}(0,\Sigma)$ where $X$ is called the reference and $\epsilon$ is the local state error with mean 0 and covariance $\Sigma$ .

We use the term extended concentrated Gaussian distribution to refer to the case where the local state error distribution has non-zero mean. For $\mu\in\mathbb{R}^{m}$ define $\xi\sim\mathbf{N}_{\phi}(X,\mu,\Sigma)$ to be

\displaystyle\xi=\phi_{X}(\varTheta_{\mathfrak{h}}^{-1}(\epsilon)).\quad\epsilon\sim\mathbf{N}(\mu,\Sigma)

(26)

One has

\displaystyle\mathrm{E}[\xi]=\phi_{X}(\varTheta_{\mathfrak{h}}(\mu)).

(27)

This extended notation allows us to explicitly model the update and reset step involving Bayesian fusion of new data and parallel transport.

6.2 Prediction step

Consider a discrete-time system (2), assume there exists a state symmetry $\phi:\mathbf{G}\times\mathcal{M}\rightarrow\mathcal{M}$ , an input symmetry $\psi:\mathbf{G}\times\mathbb{V}\rightarrow\mathbb{V}$ and an equivariant lift $\Lambda:\mathcal{M}\times\mathbb{V}\rightarrow\mathbf{G}$ . Let $\xi_{k}\in\mathcal{M}$ denote the true system state whose trajectory is determined by the external input $u_{k}\in\mathbb{V}$ . It can be expressed as a concentrated Gaussian distribution $\xi_{k}\sim\mathbf{N}_{\phi}(\hat{X}_{k},0,\Sigma_{k})$ on the homogeneous space.

Lemma 6.1.

Given a prior distribution $\xi_{k|k}\sim\mathbf{N}_{\phi}(\hat{X}_{k|k},0,\Sigma_{k|k})$ , the prediction of $\hat{X}_{k+1|k}$ and $\Sigma_{k+1|k}$ can be best approximated by

	$\displaystyle\hat{X}_{k+1\|k}$	$\displaystyle=\hat{X}_{k\|k}\Lambda(\phi_{\xi^{\circ}}(\hat{X}_{k\|k}),u_{k+1}),\quad\hat{X}_{0}=\mathrm{id}$		(28)
	$\displaystyle\Sigma_{k+1\|k}$	$\displaystyle=A_{k+1}\Sigma_{k\|k}A_{k+1}^{\top}+P,\quad\Sigma(0)=\Sigma_{0}$		(29)

where $P\in\mathbb{S}_{+}(m)$ is a constant state gain matrix.

Proof.

The log-likelihood function of $\xi_{k}$ can be written as

\displaystyle\mathcal{L}(\xi_{k|k})=\frac{1}{2}\lVert\varTheta_{\mathfrak{h}}(\phi(\hat{X}^{-1},\xi_{k|k}))\rVert^{2}_{\Sigma_{k|k}}=\frac{1}{2}\lVert\varepsilon_{k|k}\rVert^{2}_{\Sigma_{k|k}},

(30)

where $\lVert\cdot\rVert^{2}_{\Sigma}$ is the Mahalanobis norm and $\varTheta_{\mathfrak{h}}$ is a local coordinate chart on $\mathcal{M}$ . Similarly, we can derive the log-likelihood of $\xi_{k+1}$ ,

\displaystyle\mathcal{L}(\xi_{k+1|k})=\frac{1}{2}\lVert\varepsilon_{k+1|k}\rVert^{2}_{\Sigma_{k+1|k}}.

(31)

Using the linearisation of $\varepsilon_{k}$ (17), one gets

\displaystyle\mathcal{L}(\xi_{k+1|k})=\frac{1}{2}\lVert A_{k+1}\varepsilon_{k|k}\rVert^{2}_{\Sigma_{k+1|k}}.

(32)

Since there is no new information added to the filter in this stage, the probability distribution should remain the same,

	$\displaystyle\mathcal{L}(\xi_{k+1\|k})$	$\displaystyle=\mathcal{L}(F(\xi_{k\|k}))=\mathcal{L}(\xi_{k\|k});$		(33)
	$\displaystyle\frac{1}{2}\lVert A_{k+1}\varepsilon_{k\|k}\rVert^{2}_{\Sigma_{k+1\|k}}$	$\displaystyle=\frac{1}{2}\lVert\varepsilon_{k\|k}\rVert^{2}_{\Sigma_{k\|k}}.$		(34)

Solving the Mahalanobis norm yields

\displaystyle\Sigma_{k+1|k}=A_{k+1}\Sigma_{k|k}A_{k+1}^{\top},

(35)

as required. ∎

6.3 Update step

The update step involves Bayesian fusion of the predicted state estimate $\xi_{k+1|k}\sim\mathbf{N}_{\phi}(\hat{X}_{k+1|k},0,\Sigma_{k+1|k})$ with a measurement $y_{k+1}\in\mathbb{R}^{n}$ associated with a generative noise model $y_{k+1}\sim\mathbf{N}(h(\xi_{k+1}),Q)$ for $Q\in\mathbb{S}_{+}(n)$ the measurement covariance. We will solve this fusion problem in local coordinates centred at the predicted state estimate $\hat{\xi}_{k+1|k}=\phi_{\hat{X}_{k+1|k}}({\xi}^{\circ})$ . That is, consider the predicted state distribution in local coordinates

\displaystyle\Pr(\varepsilon_{k+1|k})\propto\exp(-\frac{1}{2}\varepsilon_{k+1|k}^{\top}\Sigma_{k+1|k}^{-1}\varepsilon_{k+1|k}),

(36)

and the output likelihood $l(\varepsilon_{k+1|k}|y_{k+1})\equiv\Pr(y_{k+1}|\xi_{k+1|k})$ is given by

	$\displaystyle\mathcal{L}(\epsilon_{k+1\|k}\|y_{k+1})$	$\displaystyle\propto\exp(-\frac{1}{2}(y_{k+1}-\hat{y}_{k+1})^{\top}Q^{-1}(y_{k+1}-\hat{y}_{k+1}))$
		$\displaystyle\approx\exp(-\frac{1}{2}\varepsilon_{k+1\|k}^{\top}C_{k+1\|k}^{\top}Q^{-1}C_{k+1\|k}\varepsilon_{k+1\|k}).$		(37)

Here Equation (22) is used to substitute for $\tilde{y}_{k+1}=y_{k+1}-\hat{y}_{k+1}$ and the second order error terms are ignored. The notation $C_{k+1|k}$ indicates that the linearisation is taken at the point $\hat{\xi}_{k+1|k}$ .

The probability distribution for $\Pr(\varepsilon_{k+1|k+1}|\varepsilon_{k+1|k},y_{k+1})$ , for the fused estimate $\varepsilon_{k+1|k+1}$ in local coordinates, is the Gaussian $\varepsilon_{k+1|k+1}\sim{\mathbf{N}}(\mu_{k+1},\Sigma^{\diamond}_{k+1|k+1})$ where the mean $\mu_{k+1}$ and covariance $\Sigma^{\diamond}_{k+1|k+1}$ are given by the standard update formula for Gaussian fusion

	$\displaystyle\Sigma^{\diamond}_{k+1\|k+1}$	$\displaystyle=(\Sigma_{k+1\|k}^{-1}+C_{k+1\|k}^{\top}Q^{-1}C_{k+1\|k})^{-1},$		(38)
	$\displaystyle\mu_{k+1}$	$\displaystyle=\Sigma^{\diamond}_{k+1\|k+1}C_{k+1\|k}^{\top}Q^{-1}\tilde{y}_{k+1}.$		(39)

Since this information state lives in local coordinates centred at the predicted state $\hat{\xi}_{k+1|k}$ then the updated information state can be written as an extended concentrated Gaussian

\xi_{k+1|k+1}\sim\mathbf{N}_{\phi}(\hat{X}_{k+1|k},\mu_{k+1},\Sigma^{\diamond}_{k+1|k+1}),

using the notation introduced earlier. In particular, the update step computes the new non-zero mean $\mu_{k+1}$ without changing the reference point of the coordinates $\xi_{k+1|k}$ . This leads naturally to posing the reset step as a coordinate transformation rather than a part of the stochastic fusion process.

6.4 Reset Step

In the previous subsection, the update step computed the new state estimate $\hat{\xi}_{k+1|k+1}$ in local coordinates with a non-zero mean $\mu_{k+1}$ . However, the local coordinates are still centred at $\hat{\xi}_{k+1|k}$ and the next update requires a state-estimate expressed in coordinates centred at $\hat{\xi}_{k+1|k+1}$ . The reset step computes $\hat{X}_{k+1|k+1}$ and $\Sigma_{k+1|k+1}$ such that

\mathbf{N}_{\phi}(\hat{X}_{k+1|k},\mu_{k+1},\Sigma^{\diamond}_{k+1|k+1})\approx\mathbf{N}_{\phi}(\hat{X}_{k+1|k+1},0,\Sigma_{k+1|k+1})

where $\hat{\xi}_{k+1|k}=\phi_{\hat{X}_{k+1|k}}({\xi}^{\circ})$ and $\hat{\xi}_{k+1|k+1}=\phi_{\hat{X}_{k+1|k+1}}({\xi}^{\circ})$ . Finding $\hat{X}_{k+1|k+1}$ is relatively straightforward. Computing the covariance $\Sigma_{k+1|k+1}$ is more challenging and is an active topic of research [23].

To compute $\hat{X}_{k+1|k+1}$ we use the mean $\mu_{k+1}$ of the local state error to derive a correction term on the Lie algebra. Let $\mathrm{D}\phi_{{\xi}^{\circ}}(\mathrm{id})^{\dagger}$ denote a choice of right inverse of $\mathrm{D}\phi_{{\xi}^{\circ}}(\mathrm{id})$ ; that is, $\mathrm{D}\phi_{{\xi}^{\circ}}(\mathrm{id})\mathrm{D}\phi_{{\xi}^{\circ}}(\mathrm{id})^{\dagger}:T_{{\xi}^{\circ}}\mathcal{M}\to T_{{\xi}^{\circ}}\mathcal{M}$ is the identity map. For $\mu_{k+1}\in\mathrm{T}_{\xi^{\circ}}\mathcal{M}$ define

\displaystyle\Delta_{k+1}

\displaystyle=\mathrm{D}\phi_{{\xi}^{\circ}}(\mathrm{id})^{\dagger}\mathrm{D}\varTheta_{\mathfrak{h}}^{-1}\mu_{k+1}.

(40)

The new group element is defined to be

\displaystyle\hat{X}_{k+1|k+1}:=\exp(\Delta_{k+1})\hat{X}_{k+1|k}.

(41)

By construction $\phi_{{\xi}^{\circ}}(\hat{X}_{k+1|k+1})=\hat{\xi}_{k+1|k+1}$ .

Left multiplication by the group element $\exp(\Delta_{k+1})$ can be interpreted as a left translation $\hat{X}_{k+1|k}\mapsto\hat{X}_{k+1|k+1}=\mathrm{L}_{\exp(\Delta_{k+1})}\hat{X}_{k+1|k}$ along the one-parameter subgroup $\exp(t\Delta_{k+1})$ for $t\in[0,1]$ . The Cartan-Schouten connections are a collection of three invariant connections for which the one-parameter subgroups are geodesics [26]. The covariance $\Sigma^{\diamond}_{k+1|k+1}\in\mathbb{S}_{+}(m)$ can be thought of as a (2,0)-tensor and the inverse covariance, or information matrix, $(\Sigma^{\diamond}_{k+1|k+1})^{-1}$ a (0,2)-tensor. Interpreting the curve $\exp(t\Delta_{k+1})$ as a geodesic we propose to parallel transport the covariance tensor $\Sigma^{\diamond}_{k+1|k+1}$ from the base point $\hat{\xi}_{k+1|k}$ to the new base point $\hat{\xi}_{k+1|k+1}$ along the curve $\exp(t\Delta_{k+1})$ . This approach provides a natural geometric manner to reset the covariance although it requires the additional structure of an affine connection that we discuss at the end of the section. We will use the Cartan-Schouten (0)-connection, the unique torsion-free invariant connection.

Let $v\in\mathfrak{g}$ be a tangent vector at identity and $\Delta\in\mathfrak{g}$ . The parallel transport $\mathrm{P}_{\exp(t\Delta)}(v)$ for the Cartan-Schouten (0)-connection is given by

\displaystyle\mathrm{P}_{\gamma_{\Delta}(t)}(v)=\mathrm{D}\mathrm{L}_{\exp(t\Delta)}\operatorname{Ad}_{\exp(-\frac{t}{2}\Delta)}(v).

(42)

Let $v(t)\in\mathfrak{g}$ represent the left trivialisation of $V(t)\in T_{\gamma_{\Delta}(t)}\mathbf{G}$ ; that is, $v(t):=\mathrm{D}\mathrm{L}_{\exp(-t\Delta)}V(t)$ . One has

\displaystyle v(t)=\operatorname{Ad}_{\exp(-\frac{t}{2}\Delta)}(v)=\exp(-\frac{t}{2}\Delta)\>v\>\exp(\frac{t}{2}\Delta).

(43)

Let $\Sigma$ denote the parallel transport of $\Sigma^{\diamond}$ over the curve $\exp(t\Delta)$ . One has

	$\displaystyle(\Sigma^{\diamond})^{-1}(p_{1},p_{2})=\Sigma^{-1}(\mathrm{P}_{\exp(\Delta)}(p_{1}),\mathrm{P}_{\exp(\Delta)}(p_{2}));$
	$\displaystyle p_{1}^{\top}(\Sigma^{\diamond})^{-1}p_{2}=(\operatorname{Ad}_{\exp(-\frac{1}{2}\Delta)}(p_{1}))^{\top}\Sigma^{-1}\operatorname{Ad}_{\exp(-\frac{1}{2}\Delta)}(p_{2});$
	$\displaystyle p_{1}^{\top}(\Sigma^{\diamond})^{-1}p_{2}=p_{1}^{\top}{\operatorname{Ad}_{\exp(-\frac{1}{2}\Delta)}^{\vee}}^{\top}\Sigma^{-1}\operatorname{Ad}_{\exp(-\frac{1}{2}\Delta)}^{\vee}(p_{2}),$		(44)

and hence

\Sigma_{k+1|k+1}=\operatorname{Ad}_{\exp(-\frac{1}{2}\Delta)}^{\vee}(\Sigma_{k+1|k+1}^{\diamond}){\operatorname{Ad}_{\exp(-\frac{1}{2}\Delta)}^{\vee}}^{\top}.

This formula corresponds to those obtained recently in the literature [23].

A connection, or parallel transport is a geometric structure that is in addition to the differential geometric structure inherent in the manifold structure of the Lie-group. However, the Cartan-Schouten connections are the only invariant connections for which one-parameter Lie-subgroups (exponential curves) are geodesics. The (0)-connection or normal-connection is the only torsion free Cartan-Schouten connection, although it does have curvature that is expressed in the formula for parallel transport. We believe that the torsion free property is of key importance and that the curvature is a natural consequence of the non-linear structure of the state-space.

6.5 Summary

Following the methedologies presented in this section, the Equivariant filter design for discrete-time system is

Prediction:
	$\displaystyle\hat{X}_{k+1\|k}$	$\displaystyle=\hat{X}_{k\|k}\Lambda(\phi_{\xi^{\circ}}(\hat{X}_{k\|k}),u_{k+1}),\quad\hat{X}(0)=\mathrm{id}$	(46)
	$\displaystyle\Sigma_{k+1\|k}$	$\displaystyle=A_{k+1}\Sigma_{k\|k}A_{k+1}^{\top}+P,\quad\Sigma(0)=\Sigma_{0}$	(47)
Update:
	$\displaystyle\Sigma^{\diamond}_{k+1\|k+1}$	$\displaystyle=(\Sigma_{k+1\|k}^{-1}+C_{k+1\|k}^{\top}Q^{-1}C_{k+1\|k})^{-1},$	(48)
	$\displaystyle\mu_{k+1}$	$\displaystyle=\Sigma_{k+1\|k+1}C_{k+1\|k}^{\top}Q^{-1}\tilde{y}_{k+1}$	(49)
Reset:
	$\displaystyle\Delta_{k+1}$	$\displaystyle=\mathrm{D}\phi_{{\xi}^{\circ}}(\mathrm{id})^{\dagger}\mathrm{D}\varTheta_{\mathfrak{h}}^{-1}\mu_{k+1},$	(50)
	$\displaystyle\hat{X}_{k+1\|k+1}$	$\displaystyle=\exp(\Delta_{k+1})\hat{X}_{k+1\|k},$	(51)
	$\displaystyle\Sigma_{k+1\|k+1}$	$\displaystyle=\operatorname{Ad}_{\exp(-\frac{1}{2}\Delta_{k+1})}^{\vee}\Sigma^{\diamond}_{k+1\|k+1}{\operatorname{Ad}_{\exp(-\frac{1}{2}\Delta_{k+1})}^{\vee}}^{\top}.$	(52)

Note that we group the base point reset (51) within the reset step, rather than in the update step as would be normal in a statement of the Extended Kalman Filter. This allows us to explicitly identify the updated observer state on the group separately from the local coordinates. Noting that in a classical EKF the local coordinates are one-to-one with the manifold and this distinction is unnecessary. Furthermore, in the classical EKF the underlying geometry used is flat (the local coordinates are treated as Euclidean coordinates) and the covariance update (52) is the identity.

7 EXAMPLE: SECOND-ORDER KINEMATICS

In this section, we consider the example of linear second-order kinematics in $\mathbb{R}^{3}$ with separate bearing and range measurements. Range and bearing measurements are common measurement modalities in modern robotics. These measurements could be used to reconstruct the measurement of relative position directly, however, such reconstruction distorts the uncertainties in the model, and is always better to use the raw measurements in filter design if possible.

7.1 System Definition

Second-order kinematics on $\mathbb{R}^{3}$ is given by

	$\displaystyle\dot{p}$	$\displaystyle=v,$
	$\displaystyle\dot{v}$	$\displaystyle=a,$		(53)

where the system state is $\xi=(p,v)\in\mathbb{R}^{3}\times\mathbb{R}^{3}$ , and $a\in\mathbb{R}^{3}$ is the external input. The measurement functions are given by

	$\displaystyle y_{1}:=h_{1}(p,v)=\frac{p}{\lvert p\rvert}\in\mathbf{S}^{2}$		(54)
	$\displaystyle y_{2}:=h_{2}(p,v)=\lvert p\rvert\in\mathbb{R}_{+},$		(55)

where $y_{1}$ and $y_{2}$ are bearing and range measurements respectively. In this system, the state space is $\mathcal{M}:=\mathbb{R}^{3}\times\mathbb{R}^{3}$ and output spaces are $\mathcal{N}_{1}:=\mathbf{S}^{2}\subset\mathbb{R}^{3}$ , $\mathcal{N}_{2}:=\mathbb{R}_{+}$ .

In this example, we extend the input space to $\mathbb{V}:=\mathbb{R}^{3}\times\mathbb{R}^{3}$ . An element in the input space can be thought of as two independent inputs $u=(\omega,a)\in\mathbb{V}$ , and in the implementation we will always use $u=(0,a)$ . Note that modelling $\omega\in\mathbb{R}^{3}$ as an extra input, even though it will be set to zero for all time, is critical for the equivariance of second order kinematic systems [27]. Let $t\in\mathbb{R}$ be a fixed time interval. We derive the discrete system evolution function by integrating (53) with a piecewise constant acceleration $a=a_{k+1}$ ,

	$\displaystyle p_{k+1}$	$\displaystyle=p_{k}+t(v_{k}+\omega_{k+1})+\frac{1}{2}t^{2}a_{k+1}$		(56)
	$\displaystyle v_{k+1}$	$\displaystyle=v_{k}+ta_{k+1}.$		(57)

Define the product Lie group $\mathbf{G}:=\mathbf{SO}(3)\times\mathbf{MR}(1)\ltimes\mathbb{R}^{3}$ , where $\mathbf{MR}$ is the group of positive reals under multiplication. The identity element is $(\mathrm{id}_{3},1,0)$ , and the inverse element is

\displaystyle(R,r,\beta)^{-1}=(R^{\top},\frac{1}{r},-\frac{1}{r}R^{\top}\beta).

(58)

The corresponding group multiplication is given by

\displaystyle(R_{1},r_{1},\beta_{1})(R_{2},r_{2},\beta_{2})=(R_{1}R_{2},r_{1}r_{2},\beta_{1}+r_{1}R_{1}\beta_{2}).

(59)

Define $\phi:\mathbf{G}\times\mathcal{M}\rightarrow\mathcal{M}$ by

\displaystyle\phi((R,r,\beta),(p,v_{p})):=(\frac{1}{r}R^{\top}p,\frac{1}{r}R^{\top}(v-\beta)).

(60)

Clearly $\phi$ is a smooth, transitive right group action of $\mathbf{G}$ on $\mathcal{M}$ .

7.2 Equivariant System

Given any $(\omega,a)\in\mathbb{V}$ and $(R,r,\beta)\in\mathbf{G}$ , define $\psi:\mathbf{G}\times\mathbb{V}\rightarrow\mathbb{V}$ by

\displaystyle\psi((R,r,\beta),(\omega,a)):=(\frac{1}{r}R^{\top}(\omega+\beta),\frac{1}{r}R^{\top}a).

(61)

Define the function $\Lambda:\mathcal{M}\times\mathbb{V}\rightarrow\mathbf{G}$ as

\displaystyle\Lambda((p,v),(\omega,a))=(R_{\Lambda},r_{\Lambda},\beta_{\Lambda}).

(62)

Let $p^{\prime}=p+t(v+\omega)+\frac{1}{2}t^{2}a$ , then

$\displaystyle R_{\Lambda}$	$\displaystyle=\mathrm{id}_{3}+(\frac{p^{\prime}}{\lvert p^{\prime}\rvert}\times\frac{p}{\lvert p\rvert})^{\times}+\frac{1-\frac{p^{\prime}}{\lvert p^{\prime}\rvert}^{\top}\frac{p}{\lvert p\rvert}}{\lvert\frac{p^{\prime}}{\lvert p^{\prime}\rvert}\times\frac{p}{\lvert p\rvert}\rvert^{2}}(\frac{p^{\prime}}{\lvert p^{\prime}\rvert}\times\frac{p}{\lvert p\rvert})^{\times^{2}},$	(63)
$\displaystyle r_{\Lambda}$	$\displaystyle=\frac{\lvert p\rvert}{\lvert p^{\prime}\rvert},$	(64)
$\displaystyle\beta_{\Lambda}$	$\displaystyle=v-r_{\Lambda}R_{\Lambda}(v+ta),$	(65)

where $R_{\Lambda}$ is the solution of $R_{\Lambda}\frac{p^{\prime}}{\lvert p^{\prime}\rvert}=\frac{p}{\lvert p\rvert}$ .

7.3 Implementation

We simulate an oscillatory trajectory for second-order kinematics to verify the performance of the proposed filter. The state is initialized with $(p,v)=((0,0,50),(0,0,0))\in\mathbb{R}^{3}\times\mathbb{R}^{3}$ , with accelaration $a=(0,\cos(\tau),0)$ . The trajectory is realized using Euler integration at time step $t=10^{-4}s$ , sampled at 100 Hz. The estimator has an acceleration sensor that reads $a_{k}\in\mathbb{R}^{3}$ but corrupted with piecewise constant zero-mean white Gaussian noise with variance $0.05m/s^{2}$ per axis. Additionally, the sensor provides bearing and range measurements and is also corrupted with Gaussian noises $\mu_{b}\sim\mathbf{N}(0,1^{2})$ and $\mu_{r}\sim\mathbf{N}(0,1^{2})$ in degrees and metres respectively. We implement the extended Kalman filter with linearised output, the continuous-time EqF [10] and the discrete-time EqF. For the discrete-time EqF, we follow the design procedure presented in Section 6. The local coordinate chart for the state is the normal coordinates on $\mathcal{M}$ derived from projecting exponentials from the Lie group to the manifold through the group action. The gain matrices are chosen based on the true noise parameters. All filters are initialized at $(p,v)=((0,0,50),(0,0,0))$ with Gaussian noises $\mu_{p}\sim\mathbf{N}(0,7.5^{2})$ and $\mu_{v}\sim\mathbf{N}(0,2^{2})$ in meters and meters per second respectively. The filters are running at 100 Hz.

To compare their performance, we plot the error in position and velocity, as well as the filter energy. The filter energy is $\frac{1}{m}\varepsilon^{\top}\Sigma^{-1}\varepsilon$ where $m$ is the dimension of the state. The expected value of filter energy should be 1, while smaller or larger energy indicates the filter is either under-confident or over-confident about the estimation. The first comparison is between the discrete-time EqF with and without reset covariance after the update, to demonstrate the effectiveness of the proposed method. The second comparison is among the conventional EKF, continuous-time EqF and the proposed discrete-time EqF, to show the advantage of the discrete-time filter in this example.

7.4 Simulation Results

Refer to caption — Figure 1: Comparison of discrete-time EqF implementation with/without reset step in the transient stage. The black solid line shows the discrete-time EqF without reset. The purple dashed line shows the discrete-time EqF with reset. The green horizontal line in the third subplot is for filter energy = 1.

Figure 1 shows the performance of the discrete-time EqF with (black, solid) and without (purple, dash-dot) the reset step. The EqF with reset step clearly outperforms the same filter without the reset step. The improvement occurs during the transient when the correction term in the filter is larger and the effect of parallel transport is more significant. Later in the plot ( $>0.7s$ ) the filters have converged and the effect of the reset step is less noticeable (Fig. 2). The position error is less significant than the velocity error although still appreciable. The likely reason for this lies in the fact that the position is directly observable from range and bearing measurements, while the velocity estimation depends on correlation of error encoded in the covariance estimate.

Figure 2 shows the performance of EKF (red, solid), discretized continuous-time EqF (blue, dash) and discrete-time EqF (black, solid) over the same trajectory. The continuous-time and discrete-time EqFs (both with reset modification of the Riccati) have very similar performance during the transient, while EKF (without reset modification) takes much longer to converge. The discrete-time EqF has better asymptotic performance compared to the discretized continuous-time EqF, especially in the velocity error. We hypothesise that this performance gain is due to the fact that the error incurred in a discrete state update implemented as a symmetry transform is better adapted to the discrete equivariant filter equations than the error incurred by the discretization of a continuous-time filter, even when the associated filter was based on equivariant design principles in continuous time.

8 CONCLUSIONS

This paper presents an equivariant filter design methodology for discrete-time systems on homogeneous spaces. The algorithm contains an extra reset step which explicitly computes the parallel transport of covariance matrix using the invariant affine connection. The example of second order kinematics system with range and bearing measurements is used to detail the implementation. The simulation demonstrates the convergence of discrete-time EqF as well as the improvement from the covariance reset step. It is shown that the discrete system lift brings better asymptotic performance of the filter.

References

[1] F. Bullo and A. D. Lewis, Geometric Control of Mechanical Systems, ser. Texts in Applied Mathematics. New York-Heidelberg-Berlin: Springer Verlag, 2004, vol. 49.
[2] V. Jurdjevic, Geometric control theory. Cambridge;New York, NY, USA;: Cambridge University Press, 1997, vol. 52.
[3] S. Salcudean, “A globally convergent angular velocity observer for rigid body motion,” IEEE Transactions on Automatic Control, vol. 36, no. 12, pp. 1493–1497, 1991.
[4] J. Thienel and R. Sanner, “A coupled nonlinear spacecraft attitude controller and observer with an unknown constant gyro bias and gyro noise,” IEEE Transactions on Automatic Control, vol. 48, no. 11, pp. 2011–2015, 2003.
[5] N. Aghannan and P. Rouchon, “An intrinsic observer for a class of lagrangian systems,” IEEE Transactions on Automatic Control, vol. 48, no. 6, pp. 936–945, 2003.
[6] R. Mahony, T. Hamel, and J.-M. Pflimlin, “Nonlinear complementary filters on the special orthogonal group,” IEEE Transactions on Automatic Control, vol. 53, no. 5, pp. 1203–1218, 2008.
[7] S. Bonnabel, “Left-invariant extended kalman filter and attitude estimation,” in 2007 46th IEEE Conference on Decision and Control, 2007, pp. 1027–1032.
[8] S. Bonnable, P. Martin, and E. Salaün, “Invariant extended kalman filter: theory and application to a velocity-aided attitude estimation problem,” in Proceedings of the 48h IEEE Conference on Decision and Control (CDC) held jointly with 2009 28th Chinese Control Conference, 2009, pp. 1297–1304.
[9] A. Barrau and S. Bonnabel, “The invariant extended kalman filter as a stable observer,” IEEE Transactions on Automatic Control, vol. 62, no. 4, pp. 1797–1812, 2017.
[10] P. van Goor, T. Hamel, and R. Mahony, “Equivariant filter (eqf): A general filter design for systems on homogeneous spaces,” in 2020 59th IEEE Conference on Decision and Control (CDC), 2020, pp. 5401–5408.
[11] R. Mahony, T. Hamel, and J. Trumpf, “Equivariant systems theory and observer design,” arXiv preprint arXiv:2006.08276, 2020.
[12] S.-T. Chung, “Digital aspects of nonlinear synthesis problems,” Ph.D. dissertation, 1990. [Online]. Available: https://www.proquest.com/dissertations-theses/digital-aspects-nonlinear-synthesis-problems/docview/303843416/se-2?accountid=8330
[13] P. E. Crouch and R. Grossman, “Numerical integration of ordinary differential equations on manifolds,” Journal of Nonlinear Science, vol. 3, no. 1, pp. 1–33, 1993.
[14] E. Hairer, M. Hochbruck, A. Iserles, and C. Lubich, “Geometric numerical integration,” Oberwolfach Reports, vol. 3, no. 1, pp. 805–882, 2006.
[15] N. Hori, T. Mori, and P. N. Nikiforuk, “A new perspective for discrete-time models of a continuous-time system,” in 1989 American Control Conference, 1989, pp. 1659–1664.
[16] P. S. Maybeck, Stochastic models, estimation, and control. Academic press, 1982.
[17] G. Bourmaud, R. Mégret, M. Arnaudon, and A. Giremus, “Continuous-discrete extended kalman filter on matrix lie groups using concentrated gaussian distributions,” Journal of Mathematical Imaging and Vision, vol. 51, no. 1, p. 209–228, Jan 2015.
[18] A. Barrau and S. Bonnabel, “Invariant kalman filtering,” Annual Review of Control, Robotics, and Autonomous Systems, vol. 1, pp. 237–257, 2018.
[19] C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold preintegration for real-time visual–inertial odometry,” IEEE Transactions on Robotics, vol. 33, no. 1, pp. 1–21, 2016.
[20] A. Barrau and S. Bonnabel, “Linear observed systems on groups,” Systems & Control Letters, vol. 129, pp. 36–42, 2019.
[21] R. Mahony, P. van Goor, and T. Hamel, “Observer design for nonlinear systems with equivariance,” Annual Review of Control, Robotics, and Autonomous Systems, vol. 5, no. 1, p. null, 2022. [Online]. Available: https://doi.org/10.1146/annurev-control-061520-010324
[22] F. L. Markley, “Attitude error representations for kalman filtering,” Journal of guidance, control, and dynamics, vol. 26, no. 2, pp. 311–317, 2003.
[23] M. W. Mueller, M. Hehn, and R. D’Andrea, “Covariance correction step for kalman filtering with an attitude,” Journal of Guidance, Control, and Dynamics, vol. 40, no. 9, pp. 2301–2306, 2017.
[24] K. C. Wolfe and M. Mashner, “Bayesian fusion on lie groups,” Journal of Algebraic Statistics, vol. 2, no. 1, 2011.
[25] J. Cheeger, D. G. Ebin, and D. G. Ebin, Comparison theorems in Riemannian geometry. North-Holland Amsterdam, 1975, vol. 9.
[26] K. Nomizu, “Invariant affine connections on homogeneous spaces,” American Journal of Mathematics, vol. 76, no. 1, pp. 33–65, 1954. [Online]. Available: http://www.jstor.org/stable/2372398
[27] Y. Ng, P. van Goor, T. Hamel, and R. Mahony, “Equivariant systems theory and observer design for second order kinematic systems on matrix lie groups,” in 2020 59th IEEE conference on decision and control (CDC). IEEE, 2020, pp. 4194–4199.

	$\displaystyle\hat{X}_{k+1\|k}$	$\displaystyle=\hat{X}_{k\|k}\Lambda(\phi_{\xi^{\circ}}(\hat{X}_{k\|k}),u_{k+1}),\quad\hat{X}_{0}=\mathrm{id}$		(28)
	$\displaystyle\Sigma_{k+1\|k}$	$\displaystyle=A_{k+1}\Sigma_{k\|k}A_{k+1}^{\top}+P,\quad\Sigma(0)=\Sigma_{0}$		(29)

	$\displaystyle\mathcal{L}(\xi_{k+1\|k})$	$\displaystyle=\mathcal{L}(F(\xi_{k\|k}))=\mathcal{L}(\xi_{k\|k});$		(33)
	$\displaystyle\frac{1}{2}\lVert A_{k+1}\varepsilon_{k\|k}\rVert^{2}_{\Sigma_{k+1\|k}}$	$\displaystyle=\frac{1}{2}\lVert\varepsilon_{k\|k}\rVert^{2}_{\Sigma_{k\|k}}.$		(34)