Guillaume Bourmaud
email: guillaume.bourmaud@ims-bordeaux.fr

Rémi Mégret
email: remi.megret@ims-bordeaux.fr

Marc Arnaudon
email: marc.arnaudon@math.u-bordeaux1.fr

Audrey Giremus
email: audrey.giremus@ims-bordeaux.fr

Continuous-Discrete Extended Kalman Filter on Matrix Lie Groups Using Concentrated Gaussian Distributions

Keywords: Extended Kalman Filter, Matrix Lie group, Filtering on Manifold, Stochastic Processes on Manifolds, Camera Pose Filtering

In this paper we generalize the Continuous-Discrete Extended Kalman Filter (CD-EKF) to the case where the state and the observations evolve on connected unimodular matrix Lie groups. We propose a new assumed density filter called Continuous-Discrete Extended Kalman Filter on Lie Groups (CD-LG-EKF). It is built upon a geometrically meaningful modeling of the concentrated Gaussian distribution on Lie Groups. Such a distribution is parametrized by a mean and a covariance matrix defined on the Lie group and in its associated Lie algebra respectively. Our formalism yields tractable equations for both non-linear continuous time propagation and discrete update of the distribution parameters under the assumption that the posterior distribution of the state is a concentrated Gaussian. As a side effect, we contribute to the derivation of the first and second order differential of the matrix Lie group logarithm using left connection. We also show that the CD-LG-EKF reduces to the usual CD-EKF if the state and the observations evolve on Euclidean spaces. Our approach leads to a systematic methodology for the design of filters, which is illustrated by the application to a camera pose filtering problem with observations on Lie group. In this application, the CD-LG-EKF signif-

Introduction

The differential geometry formalism has been extensively employed in a wide range of applications in the last few years [START_REF] Lui | Advances in matrix manifolds for computer vision[END_REF]. This is due to the fact that treating a constrained problem naively employing classical Euclidean space tools may cause theoretical and implementation difficulties. On the contrary, taking into account the geometry of a manifold usually leads to well-posed problems which can boost the performances of algorithms [START_REF] Absil | Optimization algorithms on matrix manifolds[END_REF].

This paper deals with the estimation of a state evolving on a manifold. There exist two major approaches to perform Bayesian filtering for a state evolving on an Euclidean space: the Kalman Filters (KF) such as (Extended KF : [START_REF] Maybeck | Stochastic Models, Estimation, and Control[END_REF], Unscented KF : [START_REF] Sarkka | On unscented Kalman filtering for state estimation of continuous-time nonlinear systems[END_REF], Cubature KF : [START_REF] Arasaratnam | Cubature Kalman filtering for continuous-discrete systems: Theory and simulations[END_REF]) and the Particle Filters (PF) [START_REF] Doucet | Sequential Monte Carlo methods in practice[END_REF]. However among those methods only a few works tried to extend them to manifolds (see Table 1). Particle Filters [START_REF] Snoussi | Particle filtering on Riemannian manifolds[END_REF] for states evolving on a Riemannian, [START_REF] Tompkins | Bayesian filtering on the Stiefel manifold[END_REF] Stiefel or [START_REF] Rentmeesters | An efficient particle filtering technique on the Grassmann manifold[END_REF] Grassmann manifolds have been proposed while an Unscented KF for a state and observations evolving on Riemannian manifolds [START_REF] Hauberg | Unscented Kalman filtering on Riemannian manifolds[END_REF] has recently been developed. In [START_REF] Bourmaud | Discrete extended Kalman filter on Lie groups[END_REF], the Extended KF was generalized to matrix Lie groups for a discrete time system. However, most physical models are described by ordinary differential equations, while measurements are usually discrete. Therefore, in this work, we focus on designing a continuous-discrete filter.

Here we consider matrix Lie Groups manifolds [START_REF] Hall | Lie Groups, Lie Algebras, and Representations An Elementary Introduction[END_REF] that form an important kind of smooth manifolds. They are particularly useful to avoid singularities when representing state spaces with either circularity or boundary issues. Typical examples include the complex unit circle with complex product S 1 , × is a one-dimensional Lie Group, as well as rotation matrices SO (3), unitary quaternions SU (2), rigid-body motion SE (3), invertible matrices GL [START_REF] Ado | The representation of Lie algebras by matrices[END_REF].

Hence in this paper we extend the Continuous-Discrete Extended Kalman Filter (CD-EKF) [START_REF] Maybeck | Stochastic Models, Estimation, and Control[END_REF] defined for a state and observations evolving on Euclidean spaces to the case of a state and observations evolving on connected unimodular matrix Lie groups. This could be interpreted as a generic constrained filtering problem [START_REF] Simon | Kalman filtering with state constraints: a survey of linear and nonlinear algorithms[END_REF] by enforcing an equality constraint taking the zero value only for matrices belonging to the group. However, in this case the state covariance matrix would be singular and would not have a meaningful interpretation.

A large number of works modeling the state on a Lie group have dealt with the attitude estimation problem. Among them the Multiplicative Extended Kalman Filter (MEKF) [START_REF] Lefferts | Kalman filtering for spacecraft attitude estimation[END_REF][START_REF] Markley | Attitude error representation for Kalman filtering[END_REF] corresponds to an ad hoc modification of the usual CD-EKF that is designed to take into account the unit constraint of a unit quaternion. Many works employ this formalism [START_REF] Mourikis | Vision-aided inertial navigation for spacecraft entry, descent, and landing[END_REF][START_REF] Hall | Quaternion attitude estimation for miniature air vehicles using a multiplicative extended Kalman[END_REF][START_REF] Trawny | Vision-aided inertial navigation for pinpoint landing using observations of mapped landmarks[END_REF][START_REF] Trawny | Indirect Kalman filter for 3d attitude estimation[END_REF]. Ad hoc modifications of the Discrete Unscented Kalman Filter have also been developed for attitude estimation [START_REF] Crassidis | Unscented filtering for spacecraft attitude estimation[END_REF][START_REF] Kraft | A quaternion-based unscented Kalman filter for orientation tracking[END_REF][START_REF] Sipos | Application of the manifold-constrained unscented Kalman filter[END_REF]. The Motor Extended Kalman Filter dedicated to the Lie group SE (3) was introduced in [START_REF] Bayro-Corrochano | The motor extended Kalman filter: A geometric approach for rigid motion estimation[END_REF] to perform rigid body motion estimation.

In [START_REF] Park | Kinematic state estimation and motion planning for stochastic nonholonomic systems using the exponential map[END_REF], irreducible unitary representation matrices are used to estimate probability density function via the group Fourier transform, for long time propagations. For small propagations, they propose a particle filter like propagation step which consists in sampling paths of the stochastic differential equation on Lie groups. The probability density function is then approximated by computing the mean and the covariance from the paths.

A closely related approach to our formalism has been recently developed [START_REF] Bonnabel | Invariant extended Kalman filter : theory and application to a velocity-aided attitude estimation problem[END_REF][START_REF] Bonnabel | Left-invariant extended Kalman filter and attitude estimation[END_REF][START_REF] Martin | Generalized multiplicative extended Kalman filter for aided attitude and heading reference system[END_REF] leading to the Invariant Extended Kalman Filter (IEKF). It is dedicated to continuous systems possessing symmetries and allows one to simultaneously guarantee that the state will remain on the Lie group and extend the domain of convergence around the so-called permanent trajectories. However, neither the issue of discrete measurements nor the problem of observations evolving on a Lie group are addressed.

In contrast, our formalism deals with a continuousdiscrete problem in a unified way for generic smooth evolution equations with the guarantee that the state model is consistent with the group manifold. Moreover, we consider observations evolving on a Lie group. Assuming the posterior distribution of the state is a concentrated Gaussian on Lie groups, we propose a tractable formulation of error propagation and update that we call Continuous-Discrete Extended Kalman Filter on Lie groups (CD-LG-EKF). Moreover, we show that the CD-LG-EKF reduces to the traditional Euclidean CD-EKF if the state and the observations evolve on Euclidean spaces. Therefore, this paper introduces a generic framework, which can be tailored to specific applications by designing the Lie Groups on which the state and the observations are defined. As a side effect, we also contribute to the derivation of the first and second order differential of the matrix Lie group logarithm using left connection which analytical expressions are essential in the derivation of our formalism.

The rest of the paper is organized as follows: Section 2 introduces our notations as well as main Lie group and Lie algebra notions. The CD-LG-EKF theory is presented in Section 3. In section 4, the first and second order differential of the matrix Lie group logarithm using left connection used in the CD-LG-EKF theory are derived. In section 5 the CD-LG-EKF methodology is illustrated on a camera pose estimation problem. Finally the conclusion and future research directions are provided in Section 5.

Preliminaries

Notations

Lie Group and Lie Algebra Notations

G ⊂ R n×n : Matrix Lie Group X, Y ∈ G : Elements of G e = Id n×n : Identity of G XY, X -1 : Group composition and inverse T X G ⊂ R n×n : Tangent space of G at X v ∈ T X G : Tangent vector at X g = T e G : Lie algebra of G a, b ∈ g, a = [a] ∨ G , b = [b] ∨ G ∈ R p : Elements of g and their vector representation [•] ∧ G : R p → g, [•] ∨ G : g → R p : Lie algebra isomorphisms L g X = gX : Left action of G onto itself with g ∈ G DL g : T X G → T gX G : Tangent mapping of L g exp G : g → G : Matrix exponential log G : G → g : Matrix logarithm
[a, b] = ab -ba : Lie Bracket Ad G (X) b = XbX -1 ∨ G : Adjoint representation of G on g ad G (a) b = [[a, b]] ∨ G : Adjoint endomorphism with ad G (a) ⊂ R p×p 2.1.2 Propagation Step Notations X (t) ∈ G : State X (t) ∼ N G (µ (t) , P (t)) : Concentrated Gaussian distribution on Lie Group µ (t) ∈ G : State mean (t) = log G µ (t) -1 X (t) ∨ G ∈ R p : Lie algebraic state error (t) ∼ N R p (m (t) = 0 p×1 , P (t)

Lie Groups and Lie Algebras

In this section we give the definitions and basic properties of Lie Groups and Lie Algebra. For a detailed description of these notions the reader is referred to [START_REF] Abbaspour | Basic Lie Theory[END_REF][START_REF] Chirikjian | Stochastic Models, Information Theory, and Lie Groups[END_REF][START_REF] Hall | Lie Groups, Lie Algebras, and Representations An Elementary Introduction[END_REF]. We focus on matrix Lie Groups since almost all Lie Groups encountered in the physical sciences are matrix groups [START_REF] Gilmore | Lie Groups, Physics, and Geometry: An Introduction for Physicists, Engineers and Chemists[END_REF] and any finite dimensional Lie group is homeomorphic to a matrix Lie group [START_REF] Ado | The representation of Lie algebras by matrices[END_REF] (Ado's theorem).

Lie Groups and Lie Algebras

A Lie Group G is a group which also has the structure of a smooth manifold such that group composition and inversion are smooth operations. We refer the reader to [START_REF] Kovk | Natural Operations in Differential Geometry[END_REF] for a detailed presentation on smooth manifold. If G is a matrix Lie group, then Y ∈ G ⊂ R n×n and its operations are matrix multiplication and inversion with the identity matrix as identity element. In this case, it is possible to define the left action L Y : G → G of the group onto itself simply as follows:

L Y X = Y X (1)
where Y, X ∈ G. Note that an Euclidean space R p is a trivial matrix Lie Group by taking the matrix embedding:

x ∈ R p → X = Id p×p x 0 1 ⊂ R (p+1)×(p+1) (2)
Since G is a smooth manifold, one can attach to every point X ∈ G a tangent space T X G. The tangent space is a vector space with the dimension equal to the dimension of the manifold. Using the left group action L Y of G onto itself a natural tangent mapping DL Y : T X G → T Y X G can be defined as follows:

DL Y v = Y v ( 3
)
where v is a matrix element of T X G, i.e v ∈ T X G ⊂ R n×n . This means that a vector v defined in a tangent space T X G can be transported in any other tangent space using DL g (see fig. 1). From this property arises the notion of left invariant vector field which associates a tangent vector v (X) ∈ T X G to each element X of the group, such that all of them can be completely determined by the value of v (e) at the identity element by transporting it around the Lie Group using DL X : v (X) = DL X v (e). Thus we focus our attention on the properties of the tangent space at the identity T e G called the Lie Algebra g.

The Lie Algebra g associated to a p-dimensional matrix Lie group is a p-dimensional vector space which may be defined by a basis consisting of real matrices E i for i = 1 . . . p. This basis defines a linear isomorphism between g and R p that we denote as follows:

[•] ∨ G : g → R p (4) [•] ∧ G : R p → g (5)
For example let a ∈ g ⊂ R n×n , then we have

[a] ∨ G = a ∈ R p . Thus [E i ] ∨ G = e i
where {e i } is the natural basis of R p and a = p i=1 a i E i with a = (a 1 , . . . , a p )

T . This will allow us to express the differential calculus of the filter computation in vector form with the minimal number of parameters p, as opposed to considering the matrices with n 2 coefficients.

Link between a Lie group and its associated Lie algebra

Exponential Mapping The link between a Lie group and its associated Lie algebra can be expressed by integration of velocities in the algebra into the group structure as follows:

ġ (t) = g (t) a g (0) = e (6)
where a ∈ g, g (t) ∈ G and ġ (t) = DL g(t) a ∈ T g(t) G is the time derivative of g (t). One can see that in the case of matrix Lie Groups, the solution to this ordinary differential equation is:

g (t) = exp G (at) (7)
Fig. 2 Illustration of matrix exponential and matrix logarithm where exp G : g → G is simply the matrix exponential (see fig. 2). Note that exp G (at 1 ) exp G (at 2 ) = exp G (a (t 1 + t 2 )) and exp G (at)

M ⊂ G ⊂ R n×n log G → ← exp G s ⊂ g ⊂ R n×n [•] ∨ G → ← [•] ∧ G S ⊂ [g] ∨ G ⊂ R p
-1 = exp G (-at). We also have the inverse mapping defined by log G which corresponds to the matrix logarithm. Note that the exponential and logarithmic mappings establish a local diffeomorphism between an open neighbourhood of 0 in g and an open neighbourhood of e in G. We also define s ⊂ g and M ⊂ G as sets on which exp G and log G are bijective functions with s an open symmetric connected subset of g. We further assume that the series of exp G and log G are converging on s and M respectively. The two previous notions are summarized in fig. 3 where

S = [s] ∨ G ⊂ R p .

Non-commutativity

The Adjoint representation of G on g is defined as the linear operator Ad G :

Ad G (X) b = X [b] ∧ G X -1 ∨ G ( 8
)
It captures properties related to commutation:

Xexp G [b] ∧ G = exp G [Ad G (X) b] ∧ G X (9)
Taking the directional derivative of XbX -1 and having defined X = exp G (a), we obtain the bilinear and anticommutative Lie bracket for a matrix Lie group:

dexp G (sa) bexp G (-sa) ds s=0 = ab -ba = [a, b] (10)
∨ G = R p as follows: [a] ∧ G , [b] ∧ G ∨ G = ad G (a) b (11)
where ad G : R p → R p×p . We also have the following property:

ad G (a) b = -ad G (b) a (12)
Note that ad 0p×1 = 0 p×p . Moreover, for a commutative group ∀a ∈ R p , ad G (a) = 0 p×p . We also introduce the following notation where (ad G (a)) ij denotes the element at the ith raw and jth column of the matrix ad G (a):

(ad G (a)) ij = L T ij a (13)
where L ij ∈ R p . Finally let us introduce the first few terms of the non-trivial Baker-Campbell-Hausdorff formula which expresses the group product directly in the algebra:

a =log G (exp G (b) exp G (c)) = b + c + 1 2 [b, c] + • • • (14)
This result can also be expressed in

[g] ∨ G = R p : a = b + c + 1 2 ad G (b) c + • • • (15)
The following related formula will be useful for our derivations:

log G exp G [-a] ∧ G exp G [a + b] ∧ G ∨ G = a + Φ G (a) b + O b 2 (16)
where

Φ G (a) = ∞ m=0 (-1) m (m + 1)! ad G (a) m (17)
is called the right Jacobian of G [START_REF] Barfoot | Associating uncertainty with three-dimensional poses for use in estimation problems[END_REF]. A few more useful properties that will be used in this paper are given in appendix A.

Concentrated Gaussian Distribution on Lie Groups

In this section we introduce the concept of concentrated Gaussian on Lie groups [START_REF] Wang | Error propagation on the Euclidean group with applications to manipulators kinematics[END_REF][START_REF] Wolfe | Bayesian fusion on Lie groups[END_REF] as a generalization of the normal distribution in Euclidean space which is used in the CD-EKF formalism. In order to define such a distribution, the considered Lie group has to be a connected [START_REF] Hall | Lie Groups, Lie Algebras, and Representations An Elementary Introduction[END_REF] unimodular [START_REF] Chirikjian | Information-theoretic inequalities on unimodular Lie groups[END_REF] matrix Lie group. Henceforth, in the rest of the paper, when referring to Lie groups, we will consider this assumption to hold.

Note that this is the case of most Lie groups of interest such as SO (3), SE (3), SL (3), R n ... From [START_REF] Wang | Error propagation on the Euclidean group with applications to manipulators kinematics[END_REF] the following distribution can be defined:

ρ (x) = αe -1 2 [log G (x)] ∨ G T P -1 [log G (x)] ∨ G ( 18
)
where α is a normalizing factor, X ∈ M ⊂ G is a random variable with probability density ρ (x), G is a p-dimensional connected unimodular matrix Lie group and P is a positive definite matrix. Probability of elements outside of M is set to zero. Let us define as follows :

=

[log G (X)] ∨ G (19)
where ∈ S ⊂ R p . When ρ (X) is tightly focused around the group identity (i.e the maximum of the eigenvalues of P is small), the distribution of can be approximated by:

∼ N R p (0 p×1 , P ) (20)
Where N R p (0 p×1 , P ) is a classical Euclidean Gaussian distribution defined on R p . It corresponds to the following approximation:

log G exp G [ ] ∧ G ∨ G (21)
which is valid only when the probability mass of in S is close to one. In this case, the distribution of X is called a concentrated Gaussian distribution on G around the identity. It can be transported around µ ∈ G using the left action of the Lie group, producing a concentrated Gaussian on G centered at µ:

X = µexp G [ ] ∧ G
where ∼ N R p (0 p×1 , P )

µ will be called the mean of X, can be seen as a Lie algebraic error of mean 0 p×1 and covariance P . Figure 4 provides a graphical interpretation of the transfer of probability distribution from to X. We also introduce the following notation for a concentrated Gaussian random variable X evolving on a Lie group:

X ∼ N G (µ, P ) (23)
Such a distribution allows us to describe the uncertainty of the state in R p while the state evolves on G and hence using Euclidean tools while being invariant w.r.t the left action of the group on itself.

As recently noted in [START_REF] Barfoot | Associating uncertainty with three-dimensional poses for use in estimation problems[END_REF], the distribution we consider in [START_REF] Kovk | Natural Operations in Differential Geometry[END_REF] and [START_REF] Bourmaud | Discrete extended Kalman filter on Lie groups[END_REF] is slightly different from the one defined in [START_REF] Wang | Error propagation on the Euclidean group with applications to manipulators kinematics[END_REF][START_REF] Wolfe | Bayesian fusion on Lie groups[END_REF]. Indeed, in [START_REF] Hall | Lie Groups, Lie Algebras, and Representations An Elementary Introduction[END_REF], α depends on x and is constant only when ρ (x) is tightly focused. On the contrary, in [START_REF] Kovk | Natural Operations in Differential Geometry[END_REF] is directly defined as a Gaussian distribution in R p and thus α is constant by definition. As it is well explained in [START_REF] Barfoot | Associating uncertainty with three-dimensional poses for use in estimation problems[END_REF], working with ( 22) is more "natural" in robotics. In practice both approaches produce the same results (see eq.( 55) in [START_REF] Barfoot | Associating uncertainty with three-dimensional poses for use in estimation problems[END_REF] and eq.( 23) in [START_REF] Wang | Nonparametric second-order theory of error propagation on motion groups[END_REF]). In this section we present the theory of the proposed CD-LG-EKF.

Problem Settings

Propagation Model

In this paper, we consider the generic formulation of a dynamic system:

dX (t) = X (t) [Ω (X (t))] ∧ G dt + p i=1 X (t) E i • dB i (t) (24)
where X (t) ∈ G is the state we wish to estimate at time t, G is a p-dimensional matrix Lie group and B (t) is a p-dimensional Brownian motion with coordinates (B 1 (t) , . . . , B p (t)), and covariance matrix R, i.e [START_REF] Said | Extrinsic mean of Brownian distributions on compact Lie groups[END_REF]). Ω (X) is a differentiable left-parametrized velocity function defined as follows:

B (t 1 ) -B (t 2 ) ∼ N R p (0 p×1 , (t 2 -t 1 ) R) with t 1 < t 2 (see
Ω (X) : G → R p (25)
Using the overdot notation for differentiation to time, we could interpret [START_REF] Lefferts | Kalman filtering for spacecraft attitude estimation[END_REF] as:

Ẋ (t) = X (t) [Ω (X (t))] ∧ G + [n (t)] ∧ G (26)
where

n (t) ∼ N R p (0 p×1 , R) is a white Gaussian noise and [n (t)] ∧ G ∈ g . Hence Ẋ (t) ∈ T X G.
The driving white noise corresponds formally to:

p i=1 X (t) E i • dB i (t) = [n (t)] ∧ G dt (27)
In the following, unless there is an ambiguity, the time parameter will be dropped for the sake of readability (i.e X (t) will be denoted by X).

Remark 1

The system considered in this paper does not need to be invariant w.r.t the (left or right) action of the Lie group as in [START_REF] Bonnabel | Invariant extended Kalman filter : theory and application to a velocity-aided attitude estimation problem[END_REF].

Remark 2 In [START_REF] Lefferts | Kalman filtering for spacecraft attitude estimation[END_REF], Ω is a function of X (t). It allows to handle complex propagation models. For example, in our experimentations, both the pose of the camera and its speed belong to the state, the speed being used as input for the propagation of the pose (see 5.2).

Remark 3 If G is an Euclidean space (24) becomes:

dX (t) = Ω (X (t)) dt + dB (28)
which is the generic CD-EKF state propagation equation with additive noise (see [START_REF] Maybeck | Stochastic Models, Estimation, and Control[END_REF]).

Observation Model

We also consider discrete measurements on a q-dimensional matrix Lie group G :

z k = h (X k ) exp G [w k ] ∧ G (29)
where

h : G → G is a differentiable function, z k ∈ G , X k ∈ G
is the state we wish to estimate at time k and

w k ∼ N R q (0 q×1 , Q k ) is a white Gaussian noise.
Remark 4 If G and G are Euclidean spaces, (29) becomes:

z k = h (X k ) + w k ( 30
)
which is the generic CD-EKF state update equation with additive noise (see [START_REF] Maybeck | Stochastic Models, Estimation, and Control[END_REF]).

Proposed Solution

We assume the state posterior distribution to be a concentrated Gaussian distribution on Lie groups: p (X k |z 1 , . . . , z l ) = N G µ k|l , P k|l , i.e:

X k|l = µ k|l exp G k|l ∧ G (31)
where k|l ∼ N R p m k|l = 0 p×1 , P k|l . We focus on l = k -1 (propagation) and l = k (update). Therefore, the aim of the CD-LG-EKF is to propagate and update the distribution parameters µ k-1|k-1 and P k-1|k-1 . In our formalism, µ is the state estimate. In order to apply the concentrated Gaussian distribution formalism (i.e the maximum of the eigenvalues of P is small), the time-step between two observations as well as the maximum of the eigenvalues of the white Gaussian noise covariance matrix considered in ( 29) are assumed to be small enough.

Propagation

We assume that the state posterior distribution at time k -1 is represented by N G µ k-1|k-1 , P k-1|k-1 . Therefore, the aim of this section is to show how to propagate µ k-1|k-1 and P k-1|k-1 between two consecutive sensor measurements. The following definition of Lie algebraic state error (t) will be useful in the rest of this section:

= [ ∧ ] ∨ G = [log G (η)] ∨ G ( 32
)
where η is defined as:

η = µ -1 X (33)
Thus, we have:

η = exp G [ ] ∧ G (34)
Moreover, we recall that:

k-1|k-1 ∼ N R p m k-1|k-1 = 0 p×1 , P k-1|k-1 (35)
Remark 5 One way to estimate µ k-1|k-1 and P k-1|k-1 is to sample paths using [START_REF] Lefferts | Kalman filtering for spacecraft attitude estimation[END_REF], in a particle filter like propagation step, and then to compute the mean and the covariance from the paths as it was proposed in [START_REF] Park | Kinematic state estimation and motion planning for stochastic nonholonomic systems using the exponential map[END_REF].

On the contrary, in this work, we are interested in getting closed form (approximated) equations both for the mean and the covariance propagations in order to extend the CD-EKF propagation formalism to Lie groups.

Although the approach of [START_REF] Park | Kinematic state estimation and motion planning for stochastic nonholonomic systems using the exponential map[END_REF] would yield more accurate results than our method for a large number of simulated paths, its computational cost would be much higher.

Remark 6

When Ω does not depend on X (t) and is either constant or an explicit function of t, [START_REF] Wang | Nonparametric second-order theory of error propagation on motion groups[END_REF] proposes to generate sample paths on small time periods. The mean and covariance is then computed for each small period of time. The final mean and covariance is obtained by the concatenation of the noisy motions. Note that the second order covariance propagation formula proposed in [START_REF] Wang | Nonparametric second-order theory of error propagation on motion groups[END_REF] was recently derived in a new manner in [START_REF] Barfoot | Associating uncertainty with three-dimensional poses for use in estimation problems[END_REF].

Mean Propagation

We choose to propagate the mean µ (t) using the state model ( 24) without noise and we will show in section 3.3.2 that using this propagation formula, the Lie algebraic state error mean m (t) = E [ (t)] remains null up to first order terms:

dµ = µ [Ω (µ)] ∧ G dt (36)
where dµ dt ∈ T µ G. Remark 7 If G is an Euclidean space, we have dµ = Ω (µ) dt [START_REF] Selig | Lie groups and Lie algebras in robotics[END_REF] which corresponds to the CD-EKF propagation of the mean.

Lie Algebraic State Error Propagation

The aim of this section is to obtain a tractable equation of d (t) which is crucial both to justify [START_REF] Sarkka | On unscented Kalman filtering for state estimation of continuous-time nonlinear systems[END_REF] and to obtain the covariance propagation expression since cov ( (t)) = P (t). From (36), we have:

d µ -1 = -µ -1 dµµ -1 (36) = -[Ω (µ)] ∧ G dtµ -1 (38)
Thus we obtain:

dη (34) = d µ -1 X = d µ -1 X + µ -1 d (X) (24),(38) = -[Ω (µ)] ∧ G dtµ -1 X +µ -1 X [Ω (X)] ∧ G dt + p i=1 XE i • dB i = -[Ω (µ)] ∧ G dtη + η [Ω (X)] ∧ G dt + η p i=1 E i • dB i = η -Ad G η -1 Ω (µ) ∧ G dt + [Ω (X)] ∧ G dt + ηdB (39)
where

dB = p i=1 E i • dB i (40)
Taking the first order Taylor expansion on Lie groups of Ω, we obtain:

Ω (X) = Ω µexp G [ ] ∧ G = Ω (µ) + F + O 2 (41)
where

F = d d Ω µexp G [ ] ∧ G | =0 (42)
We now introduce the intermediate stochastic process ξ (t) whose differential is defined as follows:

dξ = η -1 • dη (43)
From the two previous results, we can derive the expression of dξ (t):

dξ = η -1 • dη (39) = -Ad G η -1 Ω (µ) ∧ G dt + [Ω (X)] ∧ G dt + dB (34) = -Ad G exp G -[ ] ∧ G Ω (µ) ∧ G dt + [Ω (X)] ∧ G dt + dB (134),(41) = -Id p×p + ad G ( ) + O 2 Ω (µ) ∧ G dt + Ω (µ) + F + O 2 ∧ G dt + dB = ad G ( ) Ω (µ) + F + O 2 ∧ G dt + dB = [(F -ad G (Ω (µ))) ] ∧ G dt + O 2 dt + dB = L ∧ dt + O ∧ 2 dt + dB (44)
where

L ∧ = [(F -ad G (Ω (µ))) ] ∧ G (45)
In order to obtain the expression of d ∧ (t), we apply Ito's lemma in Lie groups (see [START_REF] Arnaudon | Connexions et martingales dans les groupes de Lie[END_REF]) which in this case requires the expressions of the first and second order differential of the matrix Lie group logarithm using left connection. Since their expression is not straightforward and constitute a key element in the CD-LG-EKF theory, we simply give the result here and refer the reader to section 4 for the complete proof.

d ∧ = dlog G (η) = ∇log G (η) , ηdξ + 1 2 Hess log G (η) (ηdξ, ηdξ) (85),(82) = dξ + 1 2 [ ∧ , dξ] + 1 12 [ ∧ , [ ∧ , dξ]] + 1 12 [dξ, [dξ, ∧ ]] + O ∧ 2 (dξ, dξ) + O ∧ 3 dξ (44) = L ∧ dt + dB + 1 2 [ ∧ , dB] + 1 12 [ ∧ , [ ∧ , dB]] + 1 12 [dB, [dB, ∧ ]] + O ∧ 2 dt + O ∧ 3 dB (46)
d (32),( 46
) = (F -ad G (Ω (µ))) dt + db + 1 2 ad G ( ) db + 1 12 ad G ( ) 2 db + 1 12 ad G (db) 2 +O ∧ 2 dt + O ∧ 3 db = F -ad G (Ω (µ)) + 1 12 C (R) dt + Id + 1 2 ad G ( ) + 1 12 ad G ( ) 2 db +O 2 dt + O 3 db (47)
where

db = p i=1 e i • dB i (48)
and

(C (R) dt) ij (139)
= ad G (db) dm dt

2 ij = p k=1 L T ik dbdb T L kj (
(141) = Jm (51)
where

J = F -ad G (Ω (µ)) + 1 12 C (R) (52)
Thus, m (t) remains null, up to first order terms, during the propagation since m (t 0 ) = m k-1|k-1 = 0 p×1 .

Covariance Propagation

Once again neglecting terms in O 2 dt and O 3 db, and denoting Cov ( (t)) = P (t), the covariance propagation equation associated to equation ( 47) is:

dP dt (142) = JP + P J T + R + 1 4 E ad G ( ) Rad G ( ) T + 1 12 E ad G ( ) 2 R + 1 12 RE ad G ( ) 2 T ( 53
)
where

E ad G ( ) 2 ij (139) = p k=1 L T ik P L kj (54) E ad G ( ) Rad G ( ) T ij (138) = p k=1 p l=1 R kl L T ik P L jl (55) Remark 8 If G is a Euclidean space, ad G = 0 p×p .
Then we retrieve the CD-EKF covariance propagation equation:

dP dt = FP + P F T + R (56)

Propagation Step Summary

With the initial conditions µ (t = t k-1 ) = µ k-1|k-1 and P (t = t k-1 ) = P k-1|k-1 the propagation consists in integrating [START_REF] Sarkka | On unscented Kalman filtering for state estimation of continuous-time nonlinear systems[END_REF] and (53) until time t k in order to obtain the predicted mean µ k|k-1 and covariance P k|k-1 . Hence, at the end of the propagation step, the estimated state is parametrized as follows:

X k|k-1 = X k |z 1 . . . z k-1 ∼ N G µ k|k-1 , P k|k-1 (57)

Update

The aim of this section is to update µ k|k-1 and P k|k-1 incorporating the new measurement z k .

Lie algebraic error update

Let us define the following innovation term:

zk = log G h µ k|k-1 -1 z k ∨ G (29) = log G h µ k|k-1 -1 h (X k ) exp G [w k ] ∧ G ∨ G (15) = H k k|k-1 + w k + O k|k-1 2 , w k 2 (58)
where

H k = ∂ ∂ log G h µ k|k-1 -1 h µ k|k-1 exp G [ ] ∧ G ∨ G | =0 (59)
Equation ( 58) is linear in k|k-1 which evolves on R p . Therefore, we can apply the classical update equations of the Kalman filter [START_REF] Maybeck | Stochastic Models, Estimation, and Control[END_REF] to update k|k-1 into the posterior distribution as - k|k ∼ N R p m - k|k , P - k|k where m - k|k and P - k|k can be calculated as follows:

       K k = P k|k-1 H T k H k P k|k-1 H T k + Q k -1 m - k|k = 0 p×1 + K k (z k -H k 0 p×1 ) P - k|k = (Id -K k H k ) P k|k-1 (60)
Remark 9 If G and G are Euclidean spaces, eq. ( 58) simplifies to the CD-EKF innovation term:

zk = z k -h µ k|k-1 (61)

State Reparametrization

At the end of the update step, we expect to have

X k = µ k|k exp G k|k ∧
G with E k|k = 0 p×1 (conditionally to z 1 , . . . , z k ), to satisfy the concentrated Gaussian distribution definition [START_REF] Kovk | Natural Operations in Differential Geometry[END_REF]. However we have

E - k|k = m - k|k = 0 p×1
. Hence, we perform the following reparametrization:

µ k|k = µ k|k-1 exp G m - k|k ∧ G (62)
Thus, using equation ( 16) and neglecting terms in

O - k|k 2
, we obtain:

m k|k (148) = 0 p×1 (63)
P k|k (149) = Φ G m - k|k P - k|k Φ G m - k|k T (64)
Remark 10 If G is an Euclidean space then

Φ G m - k|k = 0 p×p (65)
Thus we retrieve the CD-EKF equations:

µ k|k = µ k|k-1 + m - k|k (66)
P k|k = P - k|k (67)

Update Step Summary

At the end of the update step, the estimated state is parametrized as follows:

X k |z 1 , . . . , z k ∼ N G µ k|k , P k|k (68)
where k|k ∼ N R p m k|k = 0 p×1 , P k|k .

CD-LG-EKF Algorithm

General overview

The CD-LG-EKF algorithm is summarized below (see 3.3 and 3.4 for details):

Inputs : µ k-1|k-1 , P k-1|k-1 , z k Outputs : µ k|k , P k|k Propagation on t ∈ [t k-1 , t k ] :
Integrate the following differential equations

dµ(t) dt = µ (t) [Ω (µ (t))] ∧ G dP (t) dt = J (t) P (t) + P (t) J (t) T + R + 1 4 E ad G ( (t)) Rad G ( (t)) T + 1 12 E ad G ( (t)) 2 R + 1 12 RE ad G ( (t)) 2 T
Update :

K k = P k|k-1 H T k H k P k|k-1 H T k + Q k -1 m - k|k = K k log G h µ k|k-1 -1 z k ∨ G µ k|k = µ k|k-1 exp G m - k|k ∧ G P k|k = Φ G m - k|k (Id l×l -K k H k ) P k|k-1 Φ G m - k|k T
Algorithm 1: CD-LG-EKF

Implementation Issues

In order to implement the CD-LG-EKF algorithm, equations [START_REF] Sarkka | On unscented Kalman filtering for state estimation of continuous-time nonlinear systems[END_REF] and (53) have to be integrated during ∆t where ∆t represents the time between two measurements. However, µ (t) and P (t) evolve on G and Sym + respectively where Sym + corresponds to the Riemannian manifold of symmetric positive definite matrices. Therefore some care has to be taken while propagating them.

Mean propagation To propagate µ (t) = µ t , we employ a Lie-Euler method using small steps δt, i.e δt ∆t:

µ t+δt µ t exp G [Ω (µ t )] ∧ G δt (69)
Covariance propagation To propagate P (t), we choose the Log-Euclidean metric [START_REF] Arsigny | A log-euclidean framework for statistics on diffeomorphisms[END_REF] with the following approximation where f : Sym + → Sym:

dlogm (P ) dP (f (P )) logm (P + hf (P )) -logm (P ) h ( 70
)
where logm and expm correspond to matrix logarithm and matrix exponential respectively. Thus the covariance can be propagated as follows using small steps δt: In this section, we derive the expression of the first and second order differential of the matrix Lie group logarithm using left connection. These results are used in the CD-LG-EKF theory presented in section 3. To the best of our knowledge, it is the first time these expressions are obtained.

P t+δt expm

First and second order derivative along a path of the matrix logarithm

Let us recall the matrix logarithm definition:

log G (Id + A) = ∞ k=1 (-1) k+1 k A k = A - A 2 2 + A 3 3 + O A 4 (73)
where A ∈ R n×n and the series of log G (Id + A) is converging. The first and second order derivative along a path can be obtained as follows:

d ds log G (Id + A (s)) = dA (s) ds - 1 2 A (s) dA (s) ds + dA (s) ds A (s) + 1 3 dA (s) ds A (s) 2 + A (s) dA (s) ds A (s) +A (s) 2 dA (s) ds + O A (s) 3 dA (s) ds ( 74
)
d 2 ds 2 log G (Id + A (s)) (74) = d 2 A (s) ds 2 - 1 2 A (s) d 2 A (s) ds 2 + d 2 A (s) ds 2 A (s) - dA (
where O ( • ) (•, •) is defined in section 2.1.4.

First order differential of the matrix Lie group logarithm

Let us define:

Id + A (s) = Bexp G (sc ∧ ) (76)
where B ∈ G and c ∧ ∈ g. Then:

dA (s) ds = Bexp G (sc ∧ ) c ∧ ( 77
)
d 2 A (s) ds 2 = Bexp G (sc ∧ ) c 2 ∧ (78) dA (s) ds s=0 = Bc ∧ (79) d 2 A (s) ds 2 s=0 = Bc 2 ∧ (80)
Thus, using (74) the first order differential of the matrix Lie group logarithm is:

∇log G (B) , Bc ∧ def = d ds log G (Bexp G (sc ∧ )) | s=0 (74), (79),(80)
= Bc ∧ - 1 2 ((B -Id) Bc ∧ + Bc ∧ (B -Id)) + 1 3 Bc ∧ (B -Id) 2 + (B -Id) Bc ∧ (B -Id) + (B -Id) 2 Bc ∧ + O (B -Id) 3 Bc ∧ (81)
Setting B = exp G (b ∧ ) and using the results provided as supplementary material1 , we finally obtain:

∇log G (exp G (b ∧ )) , exp G (b ∧ ) c ∧ = Id + b ∧ + 1 2 b 2 ∧ c ∧ - 1 2 b ∧ + 3 2 b 2 ∧ c ∧ + c ∧ b ∧ + 1 2 b 2 ∧ + b ∧ c ∧ b ∧ + 1 3 c ∧ b 2 ∧ + b ∧ c ∧ b ∧ + b 2 ∧ c ∧ + O b ∧ 3 c ∧ = c ∧ + 1 2 b ∧ c ∧ - 1 2 c ∧ b ∧ + 1 12 c ∧ b 2 ∧ + 1 12 b 2 ∧ c ∧ - 2 12 b ∧ c ∧ b ∧ + O b ∧ 3 c ∧ = c ∧ + 1 2 [b ∧ , c ∧ ] + 1 12 [b ∧ , [b ∧ , c ∧ ]] + O b ∧ 3 c ∧ ( 82
)
where we used the following property of the Lie bracket (a, b ∈ g):

[a, [a, b]] = aab + baa -2aba (83)
4.3 Second order differential of the matrix Lie group logarithm using left connection Using (75), the second order differential of the matrix Lie group logarithm is:

Hess log G (B) (Bc ∧ , Bc ∧ ) def = d 2 ds 2 log G (Bexp G (sc ∧ )) | s=0
(75),( 79),(80)

= Bc 2 ∧ - 1 2 (B -Id) Bc 2 ∧ + Bc 2 ∧ (B -Id) -(Bc ∧ ) 2 + 2 3 (Bc ∧ ) 2 (B -Id) + Bc ∧ (B -Id) Bc ∧ + (B -Id) (Bc ∧ ) 2 + O (B -Id) 2 (c ∧ , c ∧ ) ( 84
)
Setting B = exp G (b ∧ ), and using the results provided as supplementary material, we finally obtain:

Hess log G (exp G (b ∧ )) (exp G (b ∧ ) c ∧ , exp G (b ∧ ) c ∧ ) = (Id + b ∧ ) c 2 ∧ - 1 2 b ∧ c 2 ∧ + c 2 ∧ b ∧ -c 2 ∧ + c ∧ b ∧ c ∧ + b ∧ c 2 ∧ + 2 3 c 2 ∧ b ∧ + c ∧ b ∧ c ∧ + b ∧ c 2 ∧ + O b ∧ 2 (c ∧ , c ∧ ) = 1 6 b ∧ c 2 ∧ + 1 6 c 2 ∧ b ∧ - 2 6 c ∧ b ∧ c ∧ + O b ∧ 2 (c ∧ , c ∧ ) = 1 6 [c ∧ , [c ∧ , b ∧ ]] + O b ∧ 2 (c ∧ , c ∧ ) (85) 5

Applicability, Results and Discussion

In this section we illustrate the applicability of the CD-

LG-EKF to the practical problem of the estimation of a camera pose. When we apply the CD-LG-EKF to a particular problem, we build the Lie groups performing a direct product between the components. Instead of using a matrix representation of the state, we use a symbolic representation for the sake of brevity. An element of the group G is denoted (•) G whereas an element of the Lie algebra g is denoted (•) g . These notations are simply a rewriting of the matrices.

Lie Groups of interest & Product of Lie Groups

Some Lie groups of interest where our theory applies are listed in Table 2.

Most of the time, the state to be estimated comprises several components evolving on different Lie Groups (ex: orientation in SO (3) and position in R 3 ). The product of Lie groups is a Lie group [START_REF] Rudkovskii | Twisted product of Lie groups[END_REF]. Thus our theory also applies directly to any state that is a combination of Lie Groups. The simplest way to handle a state comprising several components is to consider the Lie group formed by their direct product (ex: SO (3) × R 3 ). However, contrary to the case of a state evolving on an Euclidean space, others ways to combine Lie groups exist [START_REF] Rudkovskii | Twisted product of Lie groups[END_REF] (ex: semi-direct product, twisted product...). As a consequence, the estimator performances depend on the choice of the group structure. To the best of our knowledge, the question of how to choose the best group remains open and application dependent [START_REF] Saccon | Lie group projection operator approach : Optimal control on tso(3)[END_REF]. The study if symmetries which was used in [START_REF] Bonnabel | Invariant extended Kalman filter : theory and application to a velocity-aided attitude estimation problem[END_REF] may provide hints for such a choice. Hence the choice of the appropriate Lie group, as well as the choice of its associated Lie algebra basis, remain a modeling question for the practitioner.

Application of the CD-LG-EKF to a Camera Pose Filtering Problem

The camera pose filtering problem we consider deals with estimating a camera pose using a white-noise acceleration model given noisy pose measurements. It presents the originality that the velocity is not measured directly, although it is included and estimated as part of the state. It is designed as a fundamental inference module to be used in applications where only the absolute pose can be measured, such as for localization and tracking from a wearable camera that is addressed in the Dem@Care project 2 [START_REF] Wannous | Place recognition via 3d modeling for personal activity lifelog using wearable camera[END_REF].

Derivation of the filter

Motion equations Let us consider the following kinematics using engineering notations as in [START_REF] Markley | Attitude error representation for Kalman filtering[END_REF]:

           Ṙ = R [ω] ∧ SO(3) + [n R ] ∧ SO(3) ω = n ω Ṫ = v + n T v = n v (86)
where 2 Dementia Ambient Care Project: www.demcare.eu -R = R gb ∈ SO (3), represents the orientation of the global frame with respect to the camera (body) frame ω = ω b ∈ R 3 is the angular velocity vector in the body frame

-T = -→ O g O b g ∈ R 3
is the position of the camera in the global frame v = v g ∈ R 3 is the radial velocity of the camera in the global frame n R , n ω , n T and n v are (potentially correlated) white Gaussian driving noises

Measurement equations

We consider the case where a calibrated camera evolves in a known 3D world and the camera pose is estimated at fixed rate by a black box such as PnP module [START_REF] Moreno-Noguer | Accurate noniterative o(n) solution to the pnp problem[END_REF] that matches the image content to a 3D model of the world:

R z k = Rexp SO(3) (w R k ) T z k = T + w T k (87)
where

-R z k ∈ SO (3)
, represents a noisy observation of the orientation of the global frame with respect to the camera (body) frame -T z k ∈ R 3 is a noisy observation of the position of the camera in the global frame w R k and w T k are correlated white Gaussian noises State (X) and Lie group (G) definition We build the following Lie group on which the state X evolves:

G = SO (3) × R 3 × R 3 × R 3 (88)
(SO (3) properties are detailed in appendix D) which is symbolically represented by:

X =           R Id 3×3 ω 1 Id 3×3 T 1 Id 3×3 v 1           =     R ω T v     G (89)
X -1 =     R T -ω -T -v     G and X 1 X 2 =     R 1 R 2 ω 1 + ω 2 T 1 + T 2 v 1 + v 2     G (90)
Its associated Lie algebra is:

g = so (3) × R 3 × R 3 × R 3 (91)
and the following properties hold (a ∈ R 12 ):

a = a T R , a T ω , a T T , a T v T (92)
[a]

∧ G =           [a R ] ∧ SO(3) 0 3×3 a ω 0 0 3×3 a T 0 0 3×3 a v 0           =     [a R ] ∧ SO(3) a ω a T a v     g (93) exp G [a] ∧ G =      exp SO(3) [a R ] ∧ SO(3) a ω a T a v      G (94) ad G (a) = ad SO(3) (a R ) 0 9×9 0 9×9 0 9×9 (95) since SO (3) is the only non-commutative component of G. Φ G (a) = Φ SO(3) (a R ) 0 9×9 0 9×9 0 9×9 (96)
Calculations for the propagation step Given the previously defined Lie group G, we can rewrite the motion equations (86) in the form of the system considered in this paper [START_REF] Markley | Attitude error representation for Kalman filtering[END_REF] where

Ω (X) = ω T , 0 1×3 , v T , 0 1×3 T (97)
and

n = n T R , n T ω , n T T , n T v T (98)
Also,

= T R , T ω , T T , T v T ∈ R 12 (99)
The only required calculation for the propagation step is the derivative of Ω which has a simple form in our case:

F (t) = d d (t) Ω µ (t) exp G [ (t)] ∧ G | (t)=0 =     0 3×3 Id 3×3 0 3×3 0 3×3 0 3×3 0 3×3 0 3×3 0 3×3 0 3×3 0 3×3 0 3×3 Id 3×3 0 3×3 0 3×3 0 3×3 0 3×3     (100)
Observation (z) and Lie group (G ) definition We build the following Lie group on which the observations z k evolve:

G = SO (3) × R 3 (101)
which is symbolically represented by:

z k =   R z k Id 3×3 T z k 1   = R z k T z k G (102) z -1 k = R T z k -T z k G and z 1 z 2 = R z1 R z2 T z1 + T z2 G (103)
Its associated Lie algebra is:

g = so (3) × R 3 (104)
and the following properties hold (b ∈ R 6 ):

b = b T R , b T T (105) [b] ∧ G =   [b R ] ∧ SO(3) 0 3×3 b T 0   = [b R ] ∧ SO (3) b T g (106)
exp

G [b] ∧ G = exp SO(3) [b R ] ∧ SO(3) b T G (107)
Derivations for the update step Given the previously defined Lie group G , we can rewrite the measurement equations (87) in the form of the measurement equation considered in this paper [START_REF] Moreno-Noguer | Accurate noniterative o(n) solution to the pnp problem[END_REF] where

h (X k ) = AX k A T =   R k Id 3×3 T k 1   (108) A =   Id 3×3 0 3×3 0 3×1 0 3×3 0 3×1 0 3×3 0 3×1 0 3×3 0 3×3 0 3×1 Id 3×3 0 3×1 0 3×3 0 3×1 0 1×3 0 1×3 0 0 1×3 1 0 1×3 0   (109)
and

w k = w T R k , w T T k (110)
The only required calculation for the update step is:

H k = ∂ ∂ log G h µ k|k-1 -1 h µ k|k-1 exp G [ ] ∧ G ∨ G | =0 = ∂ ∂ R T | =0 = Id 3×3 0 3×3 0 3×3 0 3×3 0 3×3 0 3×3 Id 3×3 0 3×3 (111)

Trajectory Simulations

In order to evaluate our formalism on synthetic data, we need to simulate trajectories. We employ the following formalism:

X (t + δt) = X (t) exp G [Ω (X (t)) δt + δn] ∧ G (112)
where

δn ∼ N R 12 (0 12×1 , Rδt) (113)
In our simulations we take

R =     0 3×3 0 3×3 0 3×3 0 3×3 0 3×3 R ω 0 3×3 0 3×3 0 3×3 0 3×3 0 3×3 0 3×3 0 3×3 0 3×3 0 3×3 R v     (114)
where

R ω =   10 -3 0 0 0 10 -3 0 0 0 π 4 2   in ( rad /s 2 ) 2 (115)
which corresponds to a camera rotating mainly around the z-axis, and

R v =   1 0 0 0 1 0 0 0 10 -3   in ( m /s 2 ) 2 (116)
which corresponds to camera translating mainly in the xy-plane.

Measurement Simulations

Once a trajectory is simulated, we wish to create a sequence of measurements. Given a covariance matrix Q k , we can draw a sample from N R 6 (0 6×1 , Q k ) and simulate an observation using [START_REF] Moreno-Noguer | Accurate noniterative o(n) solution to the pnp problem[END_REF]. In order to give a physical interpretation to Q k and meaningful correlations between the translation and the orientation of the camera, Q k is obtained as follows for each observations. First of all, the simulated trajectory is placed in a 3D sparse environment (a 3D cube with keypoints on the faces in our case). The keypoints are independent and distributed using an isotropic normal distribution with a standard deviation of 0.1 meter. 2D points are simulated by reprojecting the 3D keypoints in the focal plane of the camera, assuming the camera is calibrated. The standard variation of the 2D points is set to 3 pixels for a pinhole camera with focal f = 600 pix and image size 1280 × 960. We estimate the camera pose C using a maximum likelihood algorithm [START_REF] Absil | Optimization algorithms on matrix manifolds[END_REF] which minimizes the reprojection error of the 3D keypoints in the image. The algorithm is initialized at the true (simulated) camera pose. Finally the covariance matrix (Q k ) of the estimated camera pose is approximated using Laplace approximation, i.e by inverting the pseudo-Hessian matrix of the reprojection error evaluated in C.

Derivation of a Continuous-Discrete Non Linear

Filter in the Embedding Space of the Lie Group with Extrinsic Constraint

In order to provide a baseline for performance comparison, we now derive a continuous-discrete formalism based on state of the art algorithms, which extrinsically takes into account the geometry of the Lie groups. The idea is to perform filtering in the embedding Euclidean space of the Lie groups and then to project to estimates back on the manifold as it was proposed in [START_REF] Simon | Kalman filtering with state constraints: a survey of linear and nonlinear algorithms[END_REF][START_REF] Davison | Monoslam: Real-time single camera slam[END_REF] for a discrete time system. Here, we adapt the propagation step to deal with our continuous time model but the update step remains unchanged.

Propagation step

We define the operators (•) v G : R n×n → R l which corresponds to the vectorization of an element of G and (•) M G : R l → R n×n the inverse of (•) v G . We also define π (•) : R n×n → G the orthogonal projection of R n×n on G. For example, in our application

x = (X) v G = R T 1 , R T 2 , R T 3 , w T , T T , v T T ∈ R 18×1 (117)
where R i stands for the i th column of R. We consider the following state equation in R l :

dx (t) = π (x (t)) M G Ω π (x (t)) M G ∧ G v G dt + π (x (t)) M G p i=1 [e i ] ∧ G • dB i (t) v G = f (x (t)) dt + g (x (t)) • db (t) (118)
where

f (x (t)) = π (x (t)) M G Ω π (x (t)) M G ∧ G v G (119)
db (t) is defined in [START_REF] Wolfe | Bayesian fusion on Lie groups[END_REF] and g (x (t)) is defined s.t

g (x (t)) • db = π (x (t)) M G p i=1 [e i ] ∧ G • dB i (t) v G (120)
The propagation equations of the first two moments (µ ∈ R l×1 and P ∈ Sym + ⊂ R l×l ) of x (t) can be approximated by (see [START_REF] Maybeck | Stochastic Models, Estimation, and Control[END_REF]):

dµ (t) dt f (µ (t)) (121)
where

F (t) = df (x) dx | x=µ(t) (123)
After having propagated the first two moments µ (t) and P (t) from t k-1 to t k , the manifold constraint is enforced through a projection step as follows:

µ k|k-1 = π µ - k|k-1 M G v G (124)
P k|k-1 = B k P - k|k-1 B T k (125)
where

B k = d π (x) M G v G dx | x=µ - k|k-1 (126)
This projection step is equivalent to performing an update step with a perfect measurement where the measurement equation corresponds to an equality constraint taking the zero value only for matrices belonging to the group (see [START_REF] Simon | Kalman filtering with state constraints: a survey of linear and nonlinear algorithms[END_REF]).

Update step

We define the operator (•)

v G : G ⊂ R m×m → R k which
corresponds to the vectorization of an element of G and (•)

M G : R k → G ⊂ R m×m the projection of R k on G .
We consider the following measurement equation:

y k = l (x k , w k ) = h (x k ) M G exp G [w k ] ∧ G v G (127)
omitting the constraints (x (t))

M G ∈ G and (y k ) M G ∈ G .
The first two moments of x k are updated using classical CD-EKF update equations [START_REF] Maybeck | Stochastic Models, Estimation, and Control[END_REF]:

K k = P k|k-1 H T k H k P k|k-1 H T k + M k Q k M T k -1 (128)
m k|k = K k y k -l µ k|k-1 , 0 (129)
µ - k|k = µ k|k-1 + m k|k (130)

P - k|k = (Id l×l -K k H k ) P k|k (131)
where

H k = ∂l (x, w) ∂x | x=µ k|k-1 ,w=0 (132)
and

M k = ∂l (x, w) ∂w | x=µ k|k-1 ,w=0 (133)
Finally µ - k|k and P - k|k are projected back on G using eq.( 124) and (125). In the rest of the paper, we refer to this filter as Continuous-Discrete Constrained Non-Linear Filter (CD-Constr-NLF).

Implementation issue Some care has to be taken while implementing the CD-Constr-NLF since the state covariance matrix P is singular. Indeed, in our problem, the covariance matrix is a 18 × 18 matrix whereas the estimated state has only 12 degrees of freedom. First of all, in order to propagate P , the numerical scheme described in (71) cannot be employed anymore since the matrix logarithm is not defined for a singular matrix. Instead, we use a 4 th order Runge Kutta verifying at each step that all the eigenvalues of P are either positive or equal to zero. Secondly, the matrix inversion in (128) is replaced by a pseudo-inverse. Finally, in order to improve the performances of the filter, we apply the projection step after each substep of the numerical scheme.

Application of a Continuous-Discrete Unscented Kalman Filter in the Embedding Space of the Lie Group with Extrinsic Constraint

In this section, we derive a Continuous-Discrete Constrained Unscented Kalman Filter (CD-Constr-UKF). In the Euclidean case, using the unscented transform (UT) [START_REF] Julier | New extension of the Kalman filter to nonlinear systems[END_REF] instead of a linearization technique significantly improves the performances of a filter (and also increases its computationnal cost). It might be interesting to compare such a filter, applied in the embedding space of the Lie group, to our approach, which is based on a linearization technique but intrinsically takes into account the geometry of the Lie group.

Propagation

Step The propagation step is performed as described in [START_REF] Singer | Continuous-discrete unscented Kalman filtering[END_REF] since their model fits (118). At the end of the propagation, we apply the same projection step as the one applied after the propagation step of the CD-Constr-NLF.

Update

Step The update step is performed as described in [START_REF] Julier | New extension of the Kalman filter to nonlinear systems[END_REF] since their model fits (127). At the end of the update step, we apply the same projection step as the one applied after the update step of the CD-Constr-NLF.

Implementation issue As in the CD-Constr-NLF case, some care has to be taken while implementing the CD-Constr-UKF since the state covariance matrix P is singular. First of all, the Cholesky decomposition which is usually used to draw the sigma points cannot be computed for a singular matrix. Instead, we use a Singular Value Decomposition and simply take the square root of the 12 largest singular values since the estimated state has only 12 degrees of freedom. Secondly, in order to improve the performances of the filter, we apply the projection step after each substep of the propagation step.

Simulation Results

We simulate trajectories as described in section 5.2.2. For each trajectory, we create a sequence of measurements as explained in section 5.2.3. An example of simulated trajectory is presented Fig. 5. For each filter, T and R are initialized using the first measurement with small variances (10 -2 ) whereas ω and v are set to zero with large variances (10 4 ). The time-step used to simulate a trajectory is ten times smaller than the time-step used to propagate the mean and the covariance of each filter between two measurements. Figure 6 reports the Root Mean Squared Error (RMSE) of each filter w.r.t sampling period (∆t). The RMSE is defined as the square root of the average of the following errors : µ T -T 2 2

(position error) and log

SO3 µ R T R ∨ SO(3) 2 2
(orientation error).

For small sampling period, the CD-LG-EKF, the CD-Constr-UKF and the CD-Constr-NLF give the same performances.

However, for a reasonable sampling rate, i.e 25 frames per seconds which is a standard camera frame-rate, the CD-LG-EKF and the CD-Constr-UKF performs significantly better than the CD-Constr-NLF.

For higher sampling period (∆t), the CD-Constr-NLF diverges while the proposed algorithm as well as the CD-Constr-UKF keep filtering the camera pose. Note that the CD-LG-EKF performs slightly better than the CD-Constr-UKF both in rotation and translation. It shows that intrinsically taking into account the geometry of the Lie group significantly increases the performances of a filter and can even provide better results than a computationally more expensive filter based on the unscented transform.

For very large sampling periods (∆t), the motion model becomes less informative but the CD-LG-EKF output is still numerically stable and tends towards the observations contrary to the CD-Constr-UKF that diverges.

Conclusion

In this paper, we proposed a new generic algorithm called Continuous-Discrete Extended Kalman Filter on connected unimodular matrix Lie Groups that generalizes the Continuous-Discrete Extended Kalman Filter to the case where the state and the measurements evolve on a matrix Lie groups. Assuming the posterior distribution is a concentrated Gaussian distribution, we showed, using the tools of the Lie group theory, how to propagate and update the distribution parameters. As a side effect, we also contributed to the derivation of the first and second order differential of the matrix Lie group logarithm using left connection which analytical expressions were necessary for the state covariance propagation. Our formalization led to a generic solution to the filtering problem of a state and observations evolving on matrix Lie groups. The systematic methodology of the CD-LG-EKF was illustrated by the application to a camera pose filtering problem where two constrained non-linear filters, based on a linearization technique and an unscented transform respectively, were outperformed. Future work will consider its use in localization and tracking from a wearable camera, but we hope its general formulation will find applications in many other contexts. where L ij ∈ R p×1 and x ∈ R p×1 . We have:

Acknowledgement

B Propagation

For the following dynamical equation where f and G are bounded and Lipschitz functions:

dx = f (x) dt + G (x) dβ ( 140
)
where β is a Brownian process with diffusion matrix Q (t), and x ∈ R m , the mean and covariance propagation equations are:

ṁx = E [f (x)] (141) Ṗx = E f (x) x T -E [f (x)] m T x + E xf (x) T -m x E f (x) T + E G (x) QG (x) T (142)
see [START_REF] Maybeck | Stochastic Models, Estimation, and Control[END_REF] Vol.2 Chap.11 Sec.6. After the Lie algebraic error update step (3.4.1) we have:

X k|k = µ k|k-1 exp G - k|k ∧ G = µ k|k-1 exp G m - k|k + r - k|k ∧ G (144)
where r - k|k ∼ N R p 0 p×1 , P - k|k . Applying (143), we obtain:

X k|k = µ k|k-1 exp G m - k|k ∧ G exp G Φ G m - k|k - k|k + O - k|k 2 ∧ G = µ k|k exp G k|k ∧ G (145)
where

µ k|k = µ k|k-1 exp G m - k|k ∧ G (

0

  p×n : p-by-n null matrix Id p×p : p-by-p identity matrix O a k (b, c) = F (a) (b, c) : Big O notation, with F (a) (b, c) a bilinear application in b and c and sup b,c with b =1, c =1 F (a) (b, c) F (a) ≤ C a where C is a constant

Fig. 1

 1 Fig. 1 Illustration of Tangent Mapping

Fig. 3

 3 Fig. 3 Summary table

Fig. 4

 4 Fig. 4 Concentrated Gaussian on Lie groups

e

  l e T m R lm dt (50) Since m (t) = E [ (t)], we have the following result neglecting terms in O 2 dt and O 3 db in (47):

Property 2 Property 3

 23 This research has received funding from the European Communitys Seventh Framework Programme (FP7/2007-2013) under grant agreement 288199 Dem@Care A Properties and Notations Property 1 First order Taylor expansion of Ad G (•):Ad G exp G [a] ∧ G = exp G (ad G (a)) = Id p×p + ad G (a) + O a 2 (134)where a ∈ R p . First order Taylor expansion of exp G (•):exp G ( ∧ ) = Id n×n + ∧ Adjoint properties: (ad G (x)) ij = L T ij x (136) ad G (x) T ij = (ad G (x)) ji = L T ji x(137)

Fig. 5 ((

 5 Fig. 5 Example of generated trajectory, measurements, CD-Constr-NLF output and CD-LG-EKF output with [x, y, z] T = T and [w x , w y , w z ] T = log SO(3) (R) ∨ SO(3)

Fig. 6 =

 6 Fig. 6 Root mean squared error for different sampling periods (∆t)

From the previous expression and neglecting terms in O - k|k 2 :D--

 2 E k|k = 0 p×1(148)P k|k = E k|k T k|k = E Φ G m - SO (3) Properties exp SO(3), log SO(3) and Φ SO(3) can be computed efficiently using Rodrigues' rotation formulae[START_REF] Selig | Lie groups and Lie algebras in robotics[END_REF] Let b ∈ R 3 then [a] ∧ SO(3) b = [b] * a where [b] * = -[b] ∧ SO(3) ad SO(3) (a) = [a] ∧ SO(3)

Table 1

 1 Categorization of state of the art approaches on Kalman and Particle filtering for a state evolving on a manifold (PF = particle filter, EKF = Extended Kalman filter, UKF = Unscented Kalman Filter)

	Approach	State Manifold	Observation Manifold	System	Filter Type
	Bonnabel et al. 2009 [10]	Lie groups	Euclidean	Continuous and possessing symmetries	EKF
	Snoussi et al. 2006 [41]	Riemannian	Euclidean	Discrete	PF
	Tompkins et al. 2007 [42]	Stiefel	Euclidean	Discrete	PF
	Hauberg et al. 2013 [20]	Riemannian	Riemannian	Discrete	UKF
	Rentmeesters et al. 2010 [32]	Grassmann	Grassmann	Discrete	PF
	Bourmaud et al. 2010 [11]	Matrix Lie groups	Matrix Lie groups	Discrete	EKF
	This paper	Matrix Lie groups	Matrix Lie groups	Continuous-Discrete	EKF

  Moreover if λ min (P t ) > λ max (αf (P t ) δt) then (P t + αf (P t ) δt) ∈ Sym + . Therefore we have the following constraint on α to remain on Sym + :

	expm	1 α	logm (P t ) + (logm (P α < dlogm (P t ) dP t (f (P t )) δt 4 First and Second Order Differential of the λ min (P t ) λ max (f (P t )) δt (72) Matrix Lie group Logarithm using Left Connection

t + αf (P t ) δt) -(1 -α) logm (P t )))

(71)

Table 2

 2 Lie Groups of interest

	Group	Example of Application
	SO (3) , SU (2)	Orientation
	SL (3)	Homography
	SE (3)	Rigid Body Transformation
	R + * , ×	Scaling Factor, Focal length...
	(R n , +)	Anything evolving on an Euclidean Space

Supplementary material and Matlab code are available at https://sites.google.com/site/guillaumebourmaud/