Polynomial Regression on Lie Groups and Application to SE(3)

Abstract

In this paper, we address the problem of estimating the position of a mobile such as a drone from noisy position measurements using the framework of Lie groups. To model the motion of a rigid body, the relevant Lie group happens to be the Special Euclidean group $SE\left(n\right)$, with $n=2$ or 3. Our work was carried out using a previously used parametric framework which derived equations for geodesic regression and polynomial regression on Riemannian manifolds. Based on this approach, our goal was to implement this technique in the Lie group $SE\left(3\right)$ context. Given a set of noisy points in $SE\left(3\right)$ representing measurements on the trajectory of a mobile, one wants to find the geodesic that best fits those points in a Riemannian least squares sense. Finally, applications to simulated data are proposed to illustrate this work. The limitations of such a method and future perspectives are discussed.

1. Introduction

Estimating the position of an aircraft in the context of air traffic management (ATM) is necessary in two situations. In the first one, the goal is to present air traffic controllers with a clean image of the aircraft positions in their sectors. In this case, only the past trajectory is known, and one wants to give the best estimate of the current position. This is the radar tracking problem that has been dealt with for decades using Kalman filtering and its extensions [1], which has recently been studied in the framework of Lie groups [2,3]. In [4,5], the symmetries of the state space seen as a manifold are used to improve estimation. In the second situation, a database of full or partial trajectories is available so that the estimate of a position may be computed using past and future measurements. The corresponding noise removal procedure is known as regression and generally relies on a simple, often local model of the trajectory whose parameters are estimated so as to minimize a least squares criterion.

When the data belong to a Euclidean space, statistical regression analyses are usually of two kinds: parametric methods such as linear and polynomial regression and non-parametric methods such as kernel-based technique, spline smoothing, and local polynomial regression. As an alternative, due to the functional nature of trajectories mapping time to position, the problem arising from the targeted application may also be described in the general framework of functional data statistics in which mobile trajectories are functional objects belonging to a given Hilbert space. Usually, in the absence of the notion of density probability in the original infinite-dimensional Hilbert space, the classical approach is to project on a finite-dimensional subspace. However, this works only if it is possible to find a suitable basis in low dimension representing well all the possible trajectories, which is not the case in the context of highly maneuvering aircraft.

Rather than considering trajectories in the state space, we may use the framework of Lie groups by studying the motion of the mobiles in another representation space. Lie groups are continuous groups of transformations such as scalings, rotations, and translations. For poses of a rigid object, the appropriate Lie group is the Special Euclidean group $SE\left(n\right)$, which has been used in the last few years in the fields of navigation [6,7,8]; robotics [9,10]; computational anatomy [11,12,13,14]; automation and control theory [15,16,17], which explains concepts involved in calculus on manifolds such as covariant derivatives and curvature; and signal processing [18,19,20], among others.

Recently, non-parametric estimation methods on Lie groups have also been explored, such as [21], in which the authors developed non-parametric kernel-based regression where the response variable is real-valued, and the Lie group-valued predictors are contaminated by measurement errors.

Parametric models have also been studied: the analogies of Riemannian kth-order polynomials with the theory of geodesics in Riemannian manifolds were studied in [22] and further generalized regarding splines on manifolds in [23]. Riemannian polynomials [24] as well as manifold-valued spline schemes based on Bézier curves [25,26] have also been investigated. Our work is guided by the applications and is situated in a parametric framework of the geodesic regression method developed in [27]. This method was extended in [24] to higher-order polynomials on Riemannian manifolds, which include Lie groups with a left or right invariant Riemannian metric. The algorithm for geodesic and polynomial regression is explicitly derived in $SO\left(3\right)$. Our goal is to follow the same idea by modeling the motion of rigid bodies and rigid transformations with the Special Euclidean group $SE\left(n\right)$, n = 2 or 3. This group, unfortunately, does not provide a bi-invariant metric (a metric may be left- or right-invariant but not both at the same time) and, thus, does not allow for regression consistent with group operations, meaning that regression on a given sample may be similar to the regression applied to the same sample either translated on the left or on the right.

In [24], geodesic and polynomial calculations were carried out using the Levi–Civita connection, the main advantage of which is that it is compatible with the metric, and the calculations are indeed performed using that connection only. In the present contribution, instead of introducing Lagrange multipliers, a more straightforward approach is taken, by reasoning in terms of differential forms (minimizing an objective function means finding the points that cancel its first differential). Then, we extend this principle to an arbitrary connection since many applications use connections other than the Levi–Civita connection. For example, Ref. [28] uses Cartan–Schouten connections to express geodesics in Lie groups as one-parameter subgroups. Similarly, $\alpha$-connections [29] are often used in statistical models on manifolds. The advantage of using any given connection can be seen in two ways: either it simplifies the writing of the problem, or it allows us to express a non-intrinsic quantity, i.e., one whose writing depends on a specific coordinate system (e.g., this is the case for polynomial curves, which are expressed differently depending on the coordinate system chosen).

The paper is organized as follows: In the second section, the elements of differential geometry necessary to lay the theoretical background are discussed. The third section presents the notion of a polynomial function in a Lie group using jet bundles. The fourth section provides a mathematical formulation for the regression problem using differential forms. Finally, several applications to simulated data are proposed. The limitations of the method are discussed, and future perspectives to improve the estimation are proposed.

2. Elements of Differential Geometry

2.1. Manifolds, Lie Groups, and Vector Bundles

We assume the reader is familiar with the definition of a manifold as a submanifold of ${\mathbb{R}}^n$ and recall the definition of an abstract manifold here:

Definition 1.

An n-dimensional smooth manifold is a second countable, Hausdorff set $\mathcal{M}$ endowed with an equivalence class of n-dimensional atlases of class $C^{\infty }$ on $\mathcal{M}$. An n-dimensional atlas of class $C^{\infty }$ on $\mathcal{M}$ is a set of pairs ${\left\{(U_i,{\varphi }_i)\right\}}_{i\in I}$ satisfying the following axioms:

• Each $U_i$ is an open subset of $\mathcal{M}$ and, $\mathcal{M}\subset {\bigcup }_i{U}_i$;
• Each ${\varphi }_i$ is a bijection from $U_i$ into an open subset ${\varphi }_i\left(U_i\right)$ of ${\mathbb{R}}^n$, and ${\varphi }_i(U_i\bigcup U_j)\subset {\mathbb{R}}^n$ is open for every i and j;
• For every pair $(i,j)$, the map ${\varphi }_j\circ {\varphi }_i^{-1}:{\varphi }_i(U_i\cap U_j)\to {\varphi }_j(U_i\cap U_j)$ is a $C^{\infty }$ diffeomorphism.

A Lie group inherits the algebraic properties of a group and the topological properties of a manifold.

Definition 2.

A Lie group G is a smooth manifold and a group such that the multiplication $m:G\times G\to G,(x,y)\mapsto x\circ y$ and the inversion $i:G\to G,x\mapsto x^{-1}$ are smooth operations.

We shall use the following notations:

• $L_g:G\to G,x\mapsto gx$ for the left translation;
• $R_g:G\to G,x\mapsto xg$ for the right translation;
• $e\in G$ for the unit element.

Definition 3.

Let $E,\mathcal{M}$ be smooth manifolds. A real vector bundle on $\mathcal{M}$ is a triple $\left(E,\pi ,\mathcal{M}\right)$ with $\pi :E\to \mathcal{M}$ a smooth onto mapping such that

(i)

For all $p\in \mathcal{M}$, ${\pi }^{-1}\left(p\right)$ is a real vector space isomorphic to ${\mathbb{R}}^k$, k a fixed integer;

(ii)

For all $p\in \mathcal{M}$, there exists an open set $U\subset \mathcal{M},p\in U$ and a diffeomorphism ${\varphi }_U:U\times {\mathbb{R}}^k\to {\pi }^{-1}\left(U\right)$ such that

• $\forall q\in U,v\in {\mathbb{R}}^k,\pi \left({\varphi }_U(q,v)\right)=q;$
• $\forall q\in U$, the mapping $v\in {\mathbb{R}}^k\mapsto {\varphi }_U(q,v)$ is an isomorphism on ${\varphi }^{-1}\left(q\right).$

Remark 1.

Definition 3 can be utilized verbatim to define vector bundles with other fields of scalars, particularly complex or quaternionic vector bundles. However, this additional generality will not be required for the purposes of this paper.

Notation 1.

A bundle $(E,\pi ,\mathcal{M})$ is often represented by its total space E along, $\pi$ being implicit.

Definition 4.

Let $U\subset \mathcal{M}$ be an open set and $(E,\pi ,\mathcal{M})$ a vector bundle on $\mathcal{M}$. A smooth mapping $s:U\to E$ such that $\forall p\in \mathcal{M},\pi \left(s\left(p\right)\right)=p$ is called a local section. When $U=\mathcal{M}$, s is designated as a global section or simply a section.

Proposition 1.

Let $U\subset \mathcal{M}$ be an open set and $(E,\pi ,\mathcal{M})$ a vector bundle on $\mathcal{M}$. The set of sections on U has the structure of a real vector space and a $C^{\infty }(U,\mathbb{R})$ module.

Notation 2.

The $C^{\infty }(U,\mathbb{R})$-module of local sections on U is designated by $\Gamma \left(U;E\right).$ When $U=\mathcal{M}$, it is simply denoted by $\Gamma \left(E\right).$

Remark 2.

One can define equivalently a vector bundle as the sheaf generated by the local sections.

Remark 3.

A vector bundle of dimension 1 is called a line bundle. Its sections are the $C^{\infty }(\mathcal{M},\mathbb{R})$-mappings.

Definition 5.

A triple $(\mathcal{M},E,\nabla )$ with $\mathcal{M}$ a smooth manifold, E a vector bundle on $\mathcal{M}$ and ∇ a Koszul connection on E is called a gauge structure.

Proposition 2.

Let $E,F$ be two vector bundles over $\mathcal{M}.$ The pointwise operations on sections $\oplus ,\otimes ,hom(·,·)$ define the respective vector bundles $E\oplus F,E\otimes F,hom(E,F).$

Proof. 

See [30], Theorem 6.2, p. 67.    □

Definition 6.

The hom bundle with $F=\mathbb{R}$ is called the dual bundle of E and is denoted by $E^{*}.$

2.2. Koszul Connections

Definition 7.

Let $(E,\pi ,\mathcal{M})$ be a vector bundle over $\mathcal{M}.$ A Koszul connection is an $\mathbb{R}$-linear mapping:

$$\nabla :\Gamma \left(E\right)\to \Gamma (T{M}^{*}\otimes E)$$

such that

$$\forall s\in \Gamma \left(E\right),\forall f\in C^{\infty }(\mathcal{M},\mathbb{R}),\nabla \left(fs\right)=df\otimes s+f\nabla s$$

(1)

A Koszul connection is often called a covariant derivative.

Remark 4.

Applying Definition 7 on a local frame $\{e_i,i=1\dots m\}$ of E yields the so-called Christoffel symbols:

$$\forall k,j\in 1,\dots ,m,i=1\dots dim\left(\mathcal{M}\right),{\nabla }_{{\partial }_i}{e}_j={\Gamma }_{ij}^k{e}_k$$

with ${\partial }_i,i=1\dots dim\left(\mathcal{M}\right)$ a basis of $TM.$ The Christoffel symbols obviously uniquely characterize the connection $\nabla .$

Remark 5.

There is no reason for a locally constant section to vanish under the action of a Koszul connection.

Definition 8.

Given a local frame of sections $\left\{e_i\right\}$, the connection forms are the one forms ${\omega }_j^i$ such that

$$\nabla e_j=e_i\otimes {\omega }_j^i$$

(2)

Proposition 3.

If $E,F$ are two vector bundles and ${\nabla }_E,{\nabla }_F$ are, respectively, connections on $E,F$, there exist two canonical connections:

$$\begin{array}{cc}\hfill & {\nabla }_E\oplus {\nabla }_F:s_E\oplus s_F\mapsto \left({\nabla }_E{s}_E\right)\oplus \left({\nabla }_F{s}_F\right),\hfill \end{array}$$

(3)

$$\begin{array}{cc}\hfill & {\nabla }_E\otimes {\nabla }_F:s_E\otimes s_F\mapsto \left({\nabla }_E{s}_E\right)\otimes s_F+s_E\otimes \left({\nabla }_F{s}_F\right).\hfill \end{array}$$

(4)

Proposition 4.

Let E be a vector bundle over $\mathcal{M}$ and let $E^{★}$ be its dual bundle. If ∇ is a Koszul connection on E, then the dual-connection ${\nabla }^{★}$ on $E^{★}$ is defined as follows:

$${\nabla }_X^{★}\left(\omega \right)\left(Y\right)=X\left(\omega \left(Y\right)\right)-\omega \left({\nabla }_{X}Y\right),X,Y\in \Gamma \left(TM\right),\omega \in \Gamma \left(E^{★}\right).$$

(5)

Definition 9

([31]). Let $f\in C^{\infty }(\mathcal{M},\mathbb{R})$, that is f is a section of the trivial bundle $\mathcal{M}\times \mathbb{R}.$ The covariant derivative of f, still denoted by $\nabla f$, is defined as:

$$\nabla f=df.$$

(6)

Propositions 3 and 4 and Definition 9 can be combined in order to obtain the covariant derivative of arbitrary tensors.

Definition 10.

Let $(E,{\pi }_E,\mathcal{M}),(F,{\pi }_F,\mathcal{N})$ be two vector bundles. A bundle morphism is a couple of mappings $\tilde{\varphi }:E\to F,\varphi :\mathcal{M}\to \mathcal{N}$ such that, for all $x\in \mathcal{M}$, the mapping $u\in {\pi }_E^{-1}\left(x\right)\to {\varphi }_F^{-1}\left(\varphi \left(x\right)\right)$ is linear, which makes the next diagram commute:

$$\begin{array}{ccccc}E& & \stackrel{\tilde{\varphi }}{\to }& F& \\ \downarrow & {\pi }_E& & \downarrow & {\pi }_F\\ \mathcal{M}& & \stackrel{\varphi }{\to }& \mathcal{N}& \end{array}$$

(7)

Definition 11.

Let $\varphi :\mathcal{M}\to \mathcal{N}$ be a smooth mapping and $(F,{\pi }_F,\mathcal{N})$ a vector bundle on $\mathcal{N}.$ The pullback bundle ${\varphi }^{*}F$ is the vector bundle on $\mathcal{M}$ generated by the local sections of the form $x\mapsto s\left(\varphi \right(x\left)\right)$ where s is a local section of $F.$ Furthermore, there exists a bundle morphism $\tilde{\varphi }:E\to F$ sending a generating section $s\circ \varphi$ to $s.$

Definition 12.

Let $\mathcal{M},\mathcal{N}$ be smooth manifolds and $\varphi :\mathcal{M}\to \mathcal{N}$ a smooth mapping. Let E be a vector bundle on $\mathcal{N}$ equipped with a Koszul connection $\nabla .$ The pullback connection of ∇ by ϕ, designated by ${\nabla }^{\varphi }$, is defined by generating sections $s\circ \varphi$ as follows:

$${\nabla }_X^{\varphi }s\circ \varphi ={\nabla }_{D\varphi X}s$$

(8)

where $X\in TM.$

Remark 6.

The quantities in Equation (8) depend only on $X\left(p\right)\in T_p\mathcal{M}$ and $T_{\varphi \left(p\right)}\mathcal{N}.$

2.3. Covariant Derivatives and Parallel Sections

Definition 13.

Let $\mathcal{M}$ be a smooth manifold, E a vector bundle on $\mathcal{M}$ and ∇ a Koszul connection on $E.$ Let $\gamma :]0,1[\to \mathcal{M}$ a smooth mapping, that is a curve on $\mathcal{M}$. The covariant derivative of $s\in \Gamma \left(E\right)$ along γ is the mapping:

$$t\in ]0,1[\to {\nabla }_1^{\gamma }s\circ \gamma \left(t\right)$$

(9)

where 1 is the unit constant vector in $]0,1[.$

Notation 3.

The covariant derivative of a section s along a curve $\gamma$ is often denoted as follows:

$${\nabla }_{\dot{\gamma }}s$$

Proposition 5.

With the same writing conventions as in Definition 13, the covariant derivative of a section $s=s^i{e}_i$ expressed in a local frame $(e_1,\dots ,e_n)$ is given as follows:

$$d{s}^i\left(\dot{\gamma }\left(t\right)\right)e_i+{\Gamma }_{ij}^k{s}^i{\dot{\gamma }}^j\left(t\right)e_n$$

(10)

Proof. 

This result is a direct application of Definition 12 with $\varphi =\gamma .$    □

Remark 7.

The first term in Equation (10) can be written by a common abuse of notation:

$${\displaystyle \frac{d}{dt}}{s}^i\left(t\right)e_i$$

(11)

where $s^i{e}_i$ stands for the section in the pullback bundle associated to $s.$

Definition 14.

Let $(\mathcal{M},E,\nabla )$ be a gauge structure and let $\gamma :]0,1[\to \mathcal{M}$ be a smooth curve. A section $s\in \Gamma \left(E\right)$ is said to be ∇-parallel along γ if

$${\nabla }_{\dot{\gamma }}s=0.$$

Remark 8.

A parallel section is, in a local frame, the solution of an ordinary differential equation (ODE):

$${\displaystyle \frac{d}{dt}}{s}^k\left(t\right)+{\Gamma }_{ij}^k{s}^i{\dot{\gamma }}^j\left(t\right)=0,k=1\dots n$$

(12)

This gives a practical way to determine a parallel section from an initial condition.

Notation 4.

The parallel transport of a section s from a to b will be denoted ${\prod }_a^{b}s$.

Given a gauge structure $(\mathcal{M},T\mathcal{M},\nabla )$, $x\in \mathcal{M}$ and a tangent vector $X\in T_x\mathcal{M}$, the next ODE locally defines a smooth curve $\gamma$:

$$\left\{\begin{array}{cc}\hfill & \dot{\gamma }\left(t\right)=s\circ \gamma \left(t\right)\hfill \\ \hfill & {\nabla }_{\dot{\gamma }}s=0\hfill \\ \hfill & \gamma \left(0\right)=x\hfill \end{array}\right.$$

(13)

Definition 15.

Let $(\mathcal{M},T\mathcal{M},\nabla )$ be a gauge structure. For any $x\in \mathcal{M}$, there exists an open neighborhood U of 0 in $T_x\mathcal{M}$ such that the curve γ defined by Equation (13) exists to time 1 for a tangent vector $X\in U.$ The mapping is as follows:

$$X\mapsto {exp}_{x}X=\gamma \left(1\right)$$

(14)

where γ is the solution curve to Equation (13) is called the exponential map.

Definition 16.

Let $(e_1,\dots ,e_n)$ be a basis of $T_x\mathcal{M}$ such that the exponential map exists for all the $e_i,i=1\dots n.$ The mapping

$$(t_1,\dots ,t_n)\in {[0,1]}^n\mapsto {exp}_x\left(\sum _{i=1}^n{t}_i{e}_i\right)$$

(15)

defines a local coordinate system in a neighborhood of x called the normal coordinates at $x.$

Remark 9.

By the very definition of a flow, the following is deduced:

$${\displaystyle \frac{d}{dt}}{exp}_{x}tX=s\left(\gamma \left(t\right)\right)$$

(16)

with $\gamma ,s$ is the solution to ODE (13). Taking the second derivative is slightly awkward since the corresponding tangent vector would lie in $TT\mathcal{M}.$ However, it makes sense to consider the covariant derivative along γ:

$${\nabla }_{\dot{\gamma }}s=0.$$

(17)

The normal coordinate system thus has a vanishing second (covariant) derivative. The generalization of this property to higher covariant derivatives will be used later as an extension of the notion of polynomial curves.

Notation 5.

A normal coordinate system at x is often denoted by $x^i,i=1\dots n$ and the associated vector fields by ${\partial }_i.$ A vector field X is said to be a coordinate vector field if it can be written as $X=a^i{\partial }_i$ with $a_i,i=1\dots n$ constant mappings [31].

Proposition 6.

Any two coordinate vector fields $X,Y$ have a vanishing commutator $[X,Y]=0.$

Proof. 

Writing $X=X^i{\partial }_i,Y=Y^i{\partial }_i$, and using the fact that $X^i,Y^i,i=1\dots n$ are constant mappings, the following is derived:

$$[X,Y]=\left(\left({\partial }_j{Y}^i\right)X^j-\left({\partial }_j{X}^i\right)Y^j\right){\partial }_i=0$$

   □

2.4. Musical Isomorphisms

In the sequel, we will often have to toggle between matching differential forms and vectors. We define the musical isomorphisms using the metric g.

Definition 17.

Let $(\mathcal{M},g)$ be a Riemannian manifold. Let $\alpha \in T^{*}\mathcal{M}$. ${\alpha }^{♯}$ is the only element of $T\mathcal{M}$ such that, for all $X\in T\mathcal{M}$,

$$\alpha \left(X\right)=g({\alpha }^{♯},X)$$

(18)

and we note it as ${\alpha }^{♯}:=♯\left(\alpha \right)$.

In a similar way, for $Y\in T\mathcal{M}$, $Y^{♭}$ is the only element of $T^{*}\mathcal{M}$ such that, for all $X\in T\mathcal{M}$,

$$Y^{♭}\left(X\right)=g(X,Y)$$

(19)

and we note it as $Y^{♭}:=♭\left(Y\right)$.

Both isomorphisms can be expressed in local coordinates:

$$\begin{array}{cc}\hfill {\alpha }^{♯}& =♯\left(\alpha \right)=♯\left({\alpha }_i{e}^i\right)=\underset{X^i}{\underbrace{g^{ij}{\alpha }_j}}{e}_i,\hfill \\ \hfill X^{♭}& =♭\left(X\right)=♭\left(X^i{e}_i\right)=\underset{{\alpha }_i}{\underbrace{g_{ij}{X}^j}}{e}^i.\hfill \end{array}$$

where $g_{ij}$ and $g^{ij}$ are, respectively, the $i,j$ coefficient of the metric matrix and the $i,j$ coefficient of the inverse of the metric matrix.

3. Manifold-Valued Polynomial Functions

Here, we provide the basic elements on how a manifold-valued polynomial function $P:I\subset \mathbb{R}\to \mathcal{M}$ can be well defined.

As a consequence of the Taylor formula, the notion of a polynomial function of degree k on ${\mathbb{R}}^n$ is defined as a smooth mapping $P:{\mathbb{R}}^n\to \mathbb{R}$ such that

$${\displaystyle \frac{{\partial }^{\left|I\right|}P}{{\partial }^{I_1}{x}_1\dots {\partial }^{I_n}{x}_n}}=0$$

for any multi-index $I=(I_1,\dots ,I_n)$ such that $\left|I\right|={\sum }_{i=1}^n{I}_i>k$.

While it is meaningful to speak about higher derivatives only for vector space-valued maps, things are more complicated for manifold-valued maps. This kind of property may hold locally, that is, for example, if a function has a finite length Taylor expansion in terms of a local coordinate system, but generally, it fails globally, due to the transition functions between local charts. Two possibilities exist to overcome this:

• Ref. [32] restricts the Hessian to the kernel of $d{f}_p$ so that the second derivative makes sense through the transition maps of the manifold;
• A possibility to bypass this [33] is to consider directly a k-th order Taylor series through jet bundles: let ${\gamma }_1,{\gamma }_2$ be two curves $I\subset \mathbb{R}\to \mathcal{M}$ such that ${\gamma }_1\left(0\right)={\gamma }_2\left(0\right)=p$ and $(U,\varphi )$ a local chart such that $\forall i\in 1\dots k$,

  $${\left.{\displaystyle \frac{d^i}{d{t}^i}}\right|}_{t=0}\varphi \circ {\gamma }_1\left(t\right)={\left.{\displaystyle \frac{d^i}{d{t}^i}}\right|}_{t=0}\varphi \circ {\gamma }_2\left(t\right)$$
  This relation is an equivalence relation in which equivalence classes are called k-jets at p, noted $J_p^k$. The space of all k-jets of the bundle $(E,\pi ,\mathcal{M})$ is the union of k-jets at p over all the points of $\mathcal{M}$.
  The set of all k-jets at p can be given a differentiable structure in a “tower-like” bundle endowed with adapted coordinates, in which

  $$\forall m\in 1,\dots ,k,\forall p\in \mathcal{M},{\pi }^m:J^m\to J^{m-1},{\left[\gamma \right]}_p^m\mapsto {\left[\gamma \right]}_p^{m-1}$$

  are projections from one level of the bundle to the one under.

$$\begin{array}{cc}{J}^m& \\ \downarrow & {\pi }^m\\ J^{m-1}& \\ \downarrow & {\pi }^{m-1}\\ \dots & \\ \downarrow & {\pi }^1\\ J^0=E& \\ \downarrow & \pi \\ \mathcal{M}& \end{array}$$

This work uses the second approach, though it will not be explicitly reminded in the sequel.

4. Optimality Conditions

In this section, the first and second-order local optimality conditions for a real-valued mapping with domain a Riemannian manifold are briefly introduced.

Lemma 1.

Let $\mathcal{M}$ be a smooth manifold and $f:M\to \mathbb{R}$ a smooth mapping. For $p\in \mathcal{M}$ to be a local minimum of $f,$ it is necessary that p be a critical point of f, which is the case if and only if the differential of f at p vanishes: $d{f}_p=0$.

Proof. 

See [34]    □

In fact, if $\gamma :]-ϵ,+ϵ[\to M$ is a smooth curve on M such that $\gamma \left(0\right)=p$, then $d{f}_p\left(\dot{\gamma }\left(0\right)\right)$ is the first-order variation of f in the direction of the tangent vector $\dot{\gamma }\left(0\right)\in T_{p}M.$ This approach can be extended by iterating the derivative.

Definition 18.

Let $\mathcal{M}$ be a smooth manifold: a retraction is a smooth map

$$R:T\mathcal{M}\to \mathcal{M},(x,v)\mapsto R_x\left(v\right)$$

such that for any curve $c:]-\epsilon ,\epsilon [\to \mathcal{M},t\mapsto R_x\left(tv\right)$, we have $c\left(0\right)=x$ and $c^{\prime }\left(0\right)=v$.

Remark 10.

The Riemannian exponential $(x,v)\mapsto {exp}_x\left(v\right)$ and the linear map $(x,v)\mapsto x+v$ are examples of a retraction.

From this, a possible algorithm for optimization on manifolds is the Riemannian gradient descent (RGD) explained in Algorithm 1, taken from [34]:

Algorithm 1: Riemannian gradient descent

where R is a retraction.

5. The Geodesic Regression Problem

5.1. Polynomial Curves

We recall that in a Lie group G with its Lie algebra $\mathfrak{g}$, there is a way to easily construct a left-invariant metric from any given inner product on $\mathfrak{g}$, which itself can be derived from the Euclidean inner product, since $\mathfrak{g}\cong {\mathbb{R}}^{dimG}$:

$$⟪{X_g,Y_g⟫}_g=〈T_g{L}_{g^{-1}}{X}_g,T_g{L}_{g^{-1}}{Y}_g〉$$

(20)

Please note that any symmetric positive-definite matrix may be used to produce different left invariant metrics:

$$⟪{X_g,Y_g⟫}_{A,g}=〈T_g{L}_{g^{-1}}A{X}_g,T_g{L}_{g^{-1}}A{Y}_g〉$$

(21)

Likewise, a right-invariant metric can also be defined. A minimizing geodesic between two points $g_0,g_1$ with respect to $⟪{·,·⟫}_g$ is a curve $\gamma :[0,1]\to G$, such that $\gamma \left(0\right)=g_0,$$\gamma \left(1\right)=g_1$n minimizing the functional

$$L\left(\gamma \right)={\displaystyle \frac{1}{2}}{\int }_0^1⟪{\dot{\gamma }\left(t\right),\dot{\gamma }\left(t\right)⟫}_{g}dt$$

(22)

Due to the left invariance of the metric, it is enough to seek a solution to the reduced problem:

$$L\left(X\right)={\displaystyle \frac{1}{2}}{\int }_0^1⟪{\dot{X}\left(t\right),\dot{X}\left(t\right)⟫}_{e}dt$$

(23)

where $X:]-ϵ,ϵ[\to \mathfrak{g}$ is a $\mathfrak{g}$-valued curve. It is well known that X satisfies the so-called Euler–Arnold equation [35]:

$$\dot{X}\left(t\right)={ad}_{X\left(t\right)}^{t}X\left(t\right)$$

(24)

From the solution of Equation (24), the original geodesic is reconstructed with the help of the left translation

$$\dot{\gamma }\left(t\right)=T_e{L}_{\gamma \left(t\right)}X\left(t\right)$$

(25)

which simplifies in a matrix Lie group as follows:

$$\gamma \left(t\right)=exp\left({\int }_0^{t}X\left(u\right)du\right)$$

(26)

Minimizing geodesics are Riemannian equivalents to straight lines and are thus the natural object to consider for extending the linear regression to a Lie group G. Furthermore, if ∇ is a connection on G, then a minimizing geodesic $\gamma$ has to satisfy the following:

$${\nabla }_{\dot{\gamma }\left(t\right)}\dot{\gamma }\left(t\right)=0$$

(27)

thus, the Riemannian equivalent to a degree k polynomial is a curve $\gamma$ such that

$${\left({\nabla }_{\dot{\gamma }\left(t\right)}\right)}^k\dot{\gamma }\left(t\right)=0$$

(28)

Here again, by the left invariance of the metric, the connection in $T_{g}G$ has to be considered only on the basis of left-invariant vector fields $T_e{L}_g{e}_1,\dots ,T_e{L}_g{e}_n$. The connection restricted to elements of the Lie algebra will be denoted $\overline{\nabla }$ in the sequel.

Applying the Koszul formula to the basis vectors $(e_1,\dots e_n)$ of $\mathfrak{g}$, the following is obtained:

$${\Gamma }_{ij}^k={\displaystyle \frac{1}{2}}\left(c_{ij}^k+c_{ki}^j-c_{jk}^i\right)$$

(29)

where the $c_{ij}^k=〈[e_i,e_j],e_k〉$ are the structure constants and the ${\Gamma }_{ij}^k,(i,j,k)\in 1,\dots ,n$ are the Christoffel symbols of the connection.

Using this form, one can compute a degree p polynomial curve X in $\mathfrak{g}$ by solving an augmented differential equation

$$v_1\left(t\right)={\nabla }_{X\left(t\right)}{v}_0\left(t\right),v_2\left(t\right)={\nabla }_{X\left(t\right)}{v}_1\left(t\right),\dots ,{\nabla }_{X\left(t\right)}{v}_{p-1}\left(t\right)=0$$

(30)

with the convention $v_0=X$. Please note that the previous system can be written in the following form:

$${\displaystyle \frac{d}{dt}}\left(\begin{array}{c}{v}_0\\ ⋮\\ v_{p-1}\end{array}\right)=F\left(v_0,\dots ,v_{p-1}\right)$$

(31)

where the vector $v_0,\dots v_{p-1}$ is in ${\mathbb{R}}^{pn}$, with n the dimension of $\mathfrak{g}$ as a real vector space.

5.2. Regression in a Lie Group

An extended notion of parametric linear regression on Riemannian manifolds, called intrinsic geodesic regression, has been independently developed in [27,36]. Let $(y_1,\dots ,y_N)$ be a set of points representing the trajectory of the mobile on a Riemannian manifold $(\mathcal{M},g)$. The idea is to find a geodesic curve $\gamma \left(t\right)$ on the manifold that best fits those points in a "linear" manner at known times $t_1,\dots ,t_N$. The estimate is found by minimizing a least squares criterion based on a Riemannian distance between the model $\gamma \left(t\right)$ and the data.

More formally, the geodesic regression model is expressed in [27] as follows:

$$Y={\mathrm{Exp}}_{\gamma \left(t\right)}\left(\epsilon \right)={\mathrm{Exp}}_{{\mathrm{Exp}}_p\left(tv\right)}\left(\epsilon \right)$$

where $\gamma \left(t\right)={\mathrm{Exp}}_p\left(tv\right)$ is the geodesic curve given by the initial conditions $\gamma \left(0\right)=p$ and $\dot{\gamma }\left(0\right)=v$, and $\epsilon$ denotes a Gaussian random variable taking values in the tangent space at $\gamma \left(t\right)$.

In [24], this geodesic regression model is generalized to a polynomial one

$$Y={\mathrm{Exp}}_{\gamma \left(t\right)}\left(\epsilon \right)$$

where the curve $\gamma \left(t\right)$ is a Riemannian polynomial of order k, as defined by Equation (28). The Riemannian least mean squares estimate is obtained by minimizing the next criterion over the decision vectors $(\gamma \left(0\right),\dot{\gamma }\left(0\right),\dots ,{\dot{\gamma }}^{\left(i\right)}\left(0\right),\dots ,{\dot{\gamma }}^{\left(k\right)}\left(0\right))$ as follows:

$$E\left(\gamma \right)={\displaystyle \frac{1}{N}}\sum _{j=1}^{N}d{\left(\gamma \left(t_j\right),y_j\right)}^2$$

(32)

where d is a geodesic distance between the model $\gamma \left(t_j\right)$ and the data $y_j$, $j=1,\dots ,N$.

This minimization problem is then performed under the constraints that the model curve $\gamma$ has, in the geodesic regression problem, a parallel first derivative, namely ${\nabla }_{\dot{\gamma }\left(t\right)}\dot{\gamma }\left(t\right)=0$, or, more generally, a k-th order polynomial constraint ${\left({\nabla }_{\dot{\gamma }\left(t\right)}\right)}^k\dot{\gamma }\left(t\right)=0$ that is uniquely solved by adding initial conditions $\gamma \left(0\right),\dot{\gamma }\left(0\right),\dots ,{\dot{\gamma }}^{\left(i\right)}\left(0\right),\dots ,{\dot{\gamma }}^{\left(k\right)}\left(0\right)$.

Using properties of translation given by Equations (23) and (25) in a Lie group, by denoting $X_1,\dots ,X_i,\dots ,X_k$ the i-th order velocities of in $\mathfrak{g}$, we obtain the “forward” polynomial equations

$$\left\{\begin{array}{cc}{\displaystyle \frac{d}{dt}}\gamma \left(t\right)\hfill & =\gamma \left(t\right)X_1\left(t\right)\hfill \\ \hfill & ⋮\hfill \\ {\displaystyle \frac{d}{dt}}{X}_i\left(t\right)\hfill & ={\overline{\nabla }}_{X_1}{X}_i\left(t\right)+X_{i+1}\left(t\right)\hfill \\ \hfill & ⋮\hfill \\ {\displaystyle \frac{d}{dt}}{X}_k\left(t\right)\hfill & ={\overline{\nabla }}_{X_1}{X}_k\left(t\right)\hfill \end{array}\right.$$

(33)

Let us now move to minimizing the criterion given by Equation (32). The next lemma is a well-known result that can be proven using normal coordinates.

Lemma 2.

Let $p,q\in M$ such that ${log}_{p}q=V\in T_{p}M$ is well defined. The differential at p of the mapping $m\mapsto d^2(m,q)$ is the 1-form ${\alpha }_q:X\in T_{p}M\mapsto 2g(X,V)$.

Proposition 7

(Variational formula [37] p. 333, Theorem B.3). Let X be a vector field depending on a parameter λ, with value at point p denoted as $X(p;\lambda ).$ Let ${\Phi }_X$ be its flow, i.e., ${\Phi }_X(t,x;\lambda )$ is the value of the integral curve of X at time t, with $\Phi (0,x;\lambda )=x$. Then, the derivative ${\partial }_{\lambda }\Phi$ of Φ with respect to λ is the solution of the following:

$${\displaystyle \frac{d}{dt}}{\partial }_{\lambda }{\Phi }_X(t,x;\lambda )={\partial }_{x}X\left(\Phi (t,x;\lambda );\lambda \right){\partial }_{\lambda }{\Phi }_X(t,x;\lambda )+{\partial }_{\lambda }X\left({\Phi }_X(t,x;\lambda ),\lambda \right)$$

(34)

with the initial condition as follows:

$${\partial }_{\lambda }{\Phi }_X(0,x;\lambda )=0.$$

(35)

Similarly, the derivative with respect to the initial condition is the solution of the following:

$${\displaystyle \frac{d}{dt}}{\partial }_x{\Phi }_X(t,x;\lambda )={\partial }_{x}X\left(\Phi (t,x;\lambda );\lambda \right){\partial }_x{\Phi }_X(t,x;\lambda )$$

(36)

with the initial condition as follows:

$${\partial }_x{\Phi }_X(0,x;\lambda )=\mathrm{Id}.$$

(37)

The variational equation can be applied to the geodesic equation ${\nabla }_{\dot{\gamma }}\dot{\gamma }=0$ by considering a perturbation in the derivative at 0 in the form $\lambda \mapsto \dot{\gamma }\left(0\right)+\lambda v$ while keeping the starting point $p=\gamma \left(0\right)$ constant. The one-parameter family of curves will be denoted by $\gamma (p,t;\lambda )$. Letting $J={\partial }_{\lambda }\gamma$, we obtain the following:

$${\displaystyle \frac{d^2}{d{t}^2}}{J}^i=-\left({\partial }_l{\Gamma }_{jk}^i\right){\dot{\gamma }}^i{\dot{\gamma }}^j{J}^l-2{\Gamma }_{jk}^i{\displaystyle \frac{d}{dt}}{J}^j{\dot{\gamma }}^k,$$

(38)

where the 2 factor is a consequence of ∇ being torsionless. A more familiar form can be obtained by expanding J on a frame $X_1\left(t\right),\dots X_n\left(t\right)$ coming from parallel transport of a basis of $T_{p}M$. In this case, letting $J\left(t\right)=J^{\alpha }\left(t\right)X_{\alpha }\left(t\right)$, Equation (38) becomes

$${\displaystyle \frac{d^2}{d{t}^2}}J+R^{\nabla }\left(J,\dot{\gamma }\right)\dot{\gamma }=0.$$

(39)

with $R^{\nabla }$ the curvature of $\nabla .$

Remark 11.

Equation (39) is valid for an arbitrary Koszul connection without torsion, even if it is a more common use in the Riemannian setting. A solution vector field J along γ is said to be a Jacobi vector field.

Remark 12.

The result is used locally before the injectivity radius of the exp function.

Going back to the original problem, which is to find a local minimum of Criterion (32), and assuming that $\gamma$ is the integral curve of a vector field X with the initial condition $\gamma \left(0\right)=p$, an application of Lemma 2 and Proposition 7 yields the next expression for the differential at $T_p\mathcal{M}$ of $E\left(\gamma \right)$ with respect to the initial conditions as follows:

$$dE:Y\in T_p\mathcal{M}\mapsto \left\{\begin{array}{cc}\hfill & {\displaystyle \frac{2}{N}}\sum _{i=1}^{N}g\left(\gamma \left(t_i\right);{\partial }_p\gamma (t_i,p).Y,{log}_{\gamma (t_i,p)}{y}_i\right),\hfill \\ \hfill & {\displaystyle \frac{2}{N}}\sum _{i=1}^{N}g\left(\gamma \left(t_i\right);{\partial }_v\gamma (t_i,p).Y,{log}_{\gamma (t_i,p)}{y}_i\right),\hfill \\ \hfill & v=\dot{\gamma }\left(0\right).\hfill \end{array}\right.$$

(40)

The Riemannian gradient at $T_{p}M$ is thus the tangent vector $Z_0\in T_p\mathcal{M}$ (resp. $Z_1\in T_p\mathcal{M}$) such that [27]

$$\forall Y\in T_p\mathcal{M},\left\{\begin{array}{cc}\hfill & g\left(p;Y,Z_0\right)={\displaystyle \frac{2}{N}}\sum _{i=1}^{N}g\left(\gamma \left(t_i\right);{\partial }_p\gamma (t_i,p).Y,{log}_{\gamma (t_i,p)}{y}_i\right),\hfill \\ \hfill & g\left(p;Y,Z_1\right)={\displaystyle \frac{2}{N}}\sum _{i=1}^{N}g\left(\gamma \left(t_i\right);{\partial }_v\gamma (t_i,p).Y,{log}_{\gamma (t_i,p)}{y}_i\right).\hfill \end{array}\right.$$

(41)

Let, for $i=1,\dots ,N$, the individual contributions of the gradient

$$\begin{array}{cc}\hfill d{E}_i^p& :=g\left(\gamma \left(t_i\right);{\partial }_p\gamma (t_i,p).Y,{log}_{\gamma (t_i,p)}{y}_i\right),\hfill \end{array}$$

(42)

$$\begin{array}{cc}\hfill d{E}_i^v& :=g\left(\gamma \left(t_i\right);{\partial }_v\gamma (t_i,p).Y,{log}_{\gamma (t_i,p)}{y}_i\right),\hfill \end{array}$$

(43)

${\partial }_{*}\gamma (t_i,p).Y$ and ${log}_{\gamma (t_i,p)}{y}_i$ both belong to $T_{\gamma \left(t_i\right)}\mathcal{M}$. As illustrated in Figure 1 (where ${\Pi }_{t_k}^0$ denotes the parallel transport from $T_{\gamma \left(t_k\right)}\mathcal{M}$ to $T_{\gamma \left(0\right)}\mathcal{M}$, and ${ϵ}_k={log}_{\gamma (t_k,p)}{y}_k$ denotes the residue at time $t_k$), we can transport them back to $T_p\mathcal{M}$, so that there are two vectors $(Z_0^i,Z_1^i)$ such that

$$\begin{array}{cc}\hfill d{E}_i^p& =g\left(\gamma \left(t_i\right);{\partial }_p\gamma (t_i,p).Y,{ϵ}_i\right)=g(p;Y,Z_0^i),\hfill \end{array}$$

(44)

$$\begin{array}{cc}\hfill d{E}_i^v& =g\left(\gamma \left(t_i\right);{\partial }_v\gamma (t_i,p).Y,{ϵ}_i\right)=g(p;Y,Z_1^i).\hfill \end{array}$$

(45)

The vector $Z_0$ (respectively, $Z_1$) can be evaluated by summing the individual contributions as follows:

$$\forall Y\in T_p\mathcal{M},\left\{\begin{array}{cc}\hfill & g\left(p;Y,Z_0\right)={\displaystyle \frac{2}{N}}\sum _{i=1}^{N}g(p;Y,Z_0^i)={\displaystyle \frac{2}{N}}g(p;Y,\sum _{i=1}^N{Z}_0^i),\hfill \\ \hfill & g\left(p;Y,Z_1\right)={\displaystyle \frac{2}{N}}\sum _{i=1}^{N}g(p;Y,Z_1^i)={\displaystyle \frac{2}{N}}g(p;Y,\sum _{i=1}^N{Z}_1^i).\hfill \end{array}\right.$$

(46)

Finding each $Z_0^i$, (respectively, $Z_1^i$) boils down to inverting the matrix ${\partial }_p\gamma (t_i,p)$ (respectively, ${\partial }_v\gamma (t_i,p)$) and then transposing it with respect to the metric. However, an easier way to do this is by parallelly transporting the residue ${log}_{\gamma (t_i,p)}{y}_i$ from $T_{\gamma \left(t_i\right)}\mathcal{M}$ back to $T_p\mathcal{M}$.

Following [24], the problem can be tackled with the introduction of the adjoint equations. While the original work was conducted with the Levi–Civita connection, the derivation of the adjoint equations can be performed mutantis mutandis using the dual-connection ${\nabla }^{*}$ as follows:  

$$\begin{array}{cc}\hfill & {\nabla }_{\dot{\gamma }}^{*}{\lambda }_i\left(t\right)=-{\lambda }_{i-1}\left(t\right),\hfill \end{array}$$

(47)

$$\begin{array}{cc}\hfill & {\nabla }_{\dot{\gamma }}^{*}{\lambda }_0\left(t\right)=-\sum _i{R}^{{\nabla }^{*}}\left(v_i\left(t\right),{\lambda }_i\left(t\right)\right)v_1\left(t\right),\hfill \end{array}$$

(48)

where the $v_i$ values are the vector fields solutions of Equation (30), and the ${\lambda }_i$ values are adjoint vector fields coupled with the $v_i$ values.

5.3. Application to SE(3)

Coming back to the original problem, which is to estimate a trajectory (i.e., a time-stamped curve) from raw positioning data belonging to $SE\left(3\right)$, we remind here some specific elements about $SE\left(n\right)$ and its Lie algebra $\mathfrak{se}\left(n\right).$

Definition 19.

The set of affine maps $f:{\mathbb{R}}^n\to {\mathbb{R}}^n$ such that $f\left(x\right)=Rx+T$ where R is a rotation matrix ($R\in SO\left(n\right)$), and T is a vector of ${\mathbb{R}}^n$ is called the Special Euclidean group $SE\left(n\right)$ and is a Lie group.

$SE\left(n\right)$ is often called the group of rigid motions, or rigid-body motions. An element $A\in SE\left(n\right)$ can be represented by a (n + 1)-by-(n + 1) matrix of the form $\left(\begin{array}{cc}R& T\\ 0& 1\end{array}\right)$ where $R\in SO\left(n\right)$ and $T\in {\mathbb{R}}^n$ It is a group, where the product of two elements of $SE\left(3\right)$ is given as follows:

$$\left(\begin{array}{cc}{R}_1& T_1\\ 0& 1\end{array}\right)\times \left(\begin{array}{cc}{R}_2& T_2\\ 0& 1\end{array}\right)=\left(\begin{array}{cc}{R}_1{R}_2& R_1{T}_2+T_1\\ 0& 1\end{array}\right)$$

and the inverse of an element is as follows:

$${\left(\begin{array}{cc}R& T\\ 0& 1\end{array}\right)}^{-1}=\left(\begin{array}{cc}{R}^T& -R^{T}T\\ 0& 1\end{array}\right)$$

We have $dim\left(SE\left(n\right)\right)={\displaystyle \frac{n(n+1)}{2}}$; therefore, $dim\left(SE\right(3\left)\right)=6.$

Definition 20.

The vector space of real (n + 1)-by-(n + 1) matrices of the form

$$X=\left(\begin{array}{cc}K& V\\ 0& 0\end{array}\right)$$

(49)

where K is a skew-symmetric n-by-n matrix and V is a vector in ${\mathbb{R}}^n$ and the Lie algebra of $SE\left(n\right).$

Remark 13.

It can be easily shown that the matrix exponential of any matrix defined by (49) belongs to $SE\left(n\right)$. It can also be proven by the exp map $\mathfrak{se}\left(n\right)\to SE\left(n\right)$ is onto but not one-to-one due to the rotation part of the matrix being the same with angles differing by an integer multiple of $2\pi$.

Since $dim\left(\mathfrak{se}\right(3\left)\right)=dim\left(SE\right(3\left)\right)=6$, a basis for $\mathfrak{se}\left(3\right)$ corresponding to the infinitesimal rotations and translations along all three axes can be written as follows:

$$\begin{array}{c}{E}_1=\left(\begin{array}{cccc}\mathbf{0}& \mathbf{0}& \mathbf{0}& 0\\ \mathbf{0}& \mathbf{0}& -\mathbf{1}& 0\\ \mathbf{0}& \mathbf{1}& \mathbf{0}& 0\\ 0& 0& 0& 0\end{array}\right),E_2=\left(\begin{array}{cccc}\mathbf{0}& \mathbf{0}& \mathbf{1}& 0\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& 0\\ -\mathbf{1}& \mathbf{0}& \mathbf{0}& 0\\ 0& 0& 0& 0\end{array}\right),E_3=\left(\begin{array}{cccc}\mathbf{0}& -\mathbf{1}& \mathbf{0}& 0\\ \mathbf{1}& \mathbf{0}& \mathbf{0}& 0\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& 0\\ 0& 0& 0& 0\end{array}\right),\\ E_4=\left(\begin{array}{cccc}0& 0& 0& \mathbf{1}\\ 0& 0& 0& \mathbf{0}\\ 0& 0& 0& \mathbf{0}\\ 0& 0& 0& 0\end{array}\right),E_5=\left(\begin{array}{cccc}0& 0& 0& \mathbf{0}\\ 0& 0& 0& \mathbf{1}\\ 0& 0& 0& \mathbf{0}\\ 0& 0& 0& 0\end{array}\right)E_6=\left(\begin{array}{cccc}0& 0& 0& \mathbf{0}\\ 0& 0& 0& \mathbf{0}\\ 0& 0& 0& \mathbf{1}\\ 0& 0& 0& 0\end{array}\right)\end{array}$$

There is a canonical isomorphism between $\mathfrak{se}\left(3\right)$ and ${\mathbb{R}}^6$ often denoted ∧ (“hat”), with its inverse being denoted ∨ (“vee”) as follows:

$$\wedge :{\mathbb{R}}^6\to \mathfrak{se}\left(3\right),\vee :\mathfrak{se}\left(3\right)\to {\mathbb{R}}^6$$

With these elements in mind, we can solve the regression. Ref. [24] gave a closed-form expression for geodesics in groups like $SO\left(3\right)$ that have a bi-invariant metric, but no such closed form can be derived for a higher-order polynomial. Unfortunately, $SE\left(3\right)$ has no such bi-invariant metric [11], and numerical integration is the only solution available, even for order 1 polynomials, i.e., geodesics.

6. Implementation

Algorithms

We now show how the solution is numerically implemented.

The algorithm was tested on several generic trajectories generated with the MatLab software, with different noise variances according to the pseudo-code presented in Algorithm 2. In this algorithm, a maneuver is defined as a change in the motion or orientation of the object, altering its speed and direction with respect to a fixed reference point. A maneuver can be as simple as a turn, a climb, a descent, or something more complex such as a loop or helix.

Note that, here, for a given maneuver, the velocity in $\mathfrak{se}\left(\mathfrak{3}\right)$ is assumed to be constant during the said maneuver; otherwise, we would have to integrate the velocity vector field over time, as in Equation (26).

The metric used is the one derived from the usual inner product in matrix spaces:

$$〈A,B〉=tr\left(A^{T}B\right)$$

We used the Levi–Civita connection for its compatibility with the metric to measure the differences between the actual, noisy, and estimated trajectories. The algorithm may easily be changed to account for another connection.

The pseudocode in Algorithm 3 describes the procedure for determining the optimal geodesic but can easily be extended to determine an optimal curve of order k.

The following factors should be noted:

• Initialization of $p=\gamma \left(0\right)$ may be carried out in several ways: naive initialization $p_0=e$ may work, but the Fréchet mean ([38]) is often a better alternative. Another possibility is to use one of the measured points;
• Equations (33) are solved in Algorithm 3 in the specific case of a geodesic, which means $k=1$ as follows: we may rewrite the forward equations using the Christoffel symbols of the connection and then solve a “large” system of ODEs.

  $$\begin{array}{ccc}\hfill \left\{\begin{array}{ccc}{\displaystyle \frac{d}{dt}}\gamma \left(t\right)\hfill & =\hfill & \gamma \left(t\right)X_1\left(t\right)\hfill \\ \hfill & ⋮\hfill & \\ {\displaystyle \frac{d}{dt}}{X}_i\left(t\right)\hfill & =\hfill & {\overline{\nabla }}_{X_1}{X}_i\left(t\right)+X_{i+1}\left(t\right)\hfill \\ \hfill & ⋮\hfill & \\ {\displaystyle \frac{d}{dt}}{X}_k\left(t\right)\hfill & =\hfill & {\overline{\nabla }}_{X_1}{X}_k\left(t\right)\hfill \end{array}\right.& \Rightarrow \hfill & \hfill \left\{\begin{array}{ccc}\gamma \left(t\right)\hfill & =\hfill & exp\left({\int }_0^t{X}_1\left(u\right)du\right)\hfill \\ \hfill & ⋮\hfill & \\ {\dot{X}}_i\left(t\right)\hfill & =\hfill & {\Gamma }_{lj}^m{X}_1^l{X}_i^j+X_{i+1}\hfill \\ \hfill & ⋮\hfill & \\ {\dot{X}}_k\left(t\right)\hfill & =\hfill & {\Gamma }_{lj}^m{X}_1^l{X}_k^j\hfill \end{array}\right.\end{array}$$

Algorithm 2: Simulated data generation

Algorithm 3: Determining the optimum geodesic

The first examples are shown in Figure 2 and Figure 3. We can see that the estimate is close to the actual trajectory, even though a slightly curving move is present due to the estimated rotation part of the trajectory in $SE\left(3\right)$ being close to zero but not exactly zero. This could be corrected by post-processing by the flagging of the trajectory.

Figure 4 displays a trajectory that is highly unlikely for a civilian aircraft but could be plausible for a UAV. This kind of trajectory would be difficult to track by a Kalman filter unless the state variables also lie on a Lie group. Also, turn maneuvers were tested as shown in Figure 5 and Figure 6.

We can measure how well the model fits the data using a coefficient of determination $R^2$. Following [27], since the data do not belong to an Euclidean space, we will have to use the Fréchet variance as the denominator as follows:

$${\mathrm{Var}}_F={\displaystyle \frac{1}{N}}\underset{y\in \mathcal{M}}{min}\sum _{j}d{(y,y_j)}^2,$$

leading to

$$R^2=1-{\displaystyle \frac{SSE}{\mathrm{sample}\mathrm{Fr}é\mathrm{chet}\mathrm{variance}}}=1-{\displaystyle \frac{{\sum }_{j}d{(\gamma \left(t_j\right),y_j)}^2}{{min}_{y\in \mathcal{M}}{\sum }_{j}d{(y,y_j)}^2}}$$

(50)

Note that since we use simulated data, we can also measure the $R^2$ coefficient with respect to the “real” data:

$$R^2=1-{\displaystyle \frac{{\sum }_{j}d{(\gamma \left(t_j\right),y_j^{\mathrm{real}})}^2}{{min}_{y\in \mathcal{M}}{\sum }_{j}d{(y,y_j^{\mathrm{real}})}^2}}$$

(51)

Similarly to the Euclidean case, an $R^2$ close to 0 will indicate that the estimator does not provide a better estimate than the sample mean (Fréchet mean for manifold-valued data), whereas an $R^2$ equal to 1 will indicate a perfect fit between the model and the data.

Some of the results obtained from the simulations are provided in

Table 1. Results after simulations.

Maneuver	Noise Std	Nmeas	${\mathit{R}}^2$
Straight line	${10}^{-3}$	25	1
Straight line	${10}^{-2}$	25	0.99
Straight line	${10}^{-1}$	25	0.84
Straight line	${10}^{-2}$	25	0.99
Straight line	${10}^{-2}$	15	0.99
Straight line	${10}^{-2}$	10	0.99
Straight line	${10}^{-1}$	25	0.84
Straight line	${10}^{-1}$	15	0.82
Straight line	${10}^{-1}$	10	0.81
Gradual turn	${10}^{-2}$	25	0.99
Gradual turn	${10}^{-2}$	15	0.99
Gradual turn	${10}^{-2}$	10	0.99
Gradual turn	${10}^{-1}$	25	0.79
Gradual turn	${10}^{-1}$	15	0.75
Gradual turn	${10}^{-1}$	10	0.74
Sharp turn	${10}^{-1}$	25	0.92
Sharp turn	${10}^{-1}$	15	0.89
Sharp turn	${10}^{-1}$	10	0.81

Note: the bold lines are illustrated by images.

.

We notice that the algorithm performs rather well even with “sharp” maneuvers, even though the performance degrades when the noise variance increases, which makes total sense. The algorithm seems to perform well even with a small sample size such as 10 points. The nature of the maneuver does not seem to affect the performance. The source code and more data sets are available as Supplementary Material.

7. Conclusions and Future Work

In this work, we have written polynomial regression in Lie groups as a solution of a k-times-iterated covariant derivative. For that, the iterated connection is not necessarily the Levi–Civita connection. The solution was tested in the specific case of the Special Euclidean group $SE\left(3\right)$ that models rigid-body motions.

From that, several possibilities may be pursued. A theoretical study of the performance of the estimator in terms of convergence could be undertaken. Also, since the connection exponential defines a local coordinate system, we can go further and find coordinate systems that flatten the manifold, thus greatly simplifying the regression problem and having it behave as in the Euclidean case. Further work would consist of characterizing the statistical quantities associated with the estimation process using polynomial regression. Determining the limit law to be considered in this case is a problem in itself. Another axis would involve using local G-valued polynomials to track the changes in maneuvers and adapt the parameters of the optimum fitting curve.