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

Johan Aubray, Florence Nicol

<figure><img src="image_1.png" /></figure>

## To cite this version:

Johan Aubray, Florence Nicol. Polynomial Regression on Lie Groups and Application to SE(3). Entropy, 2024, 26 (10), pp.825. 10.3390/e26100825. hal-04747804

HAL Id: hal-04747804

https://hal.science/hal-04747804v1

Submitted on 6 Nov 2024

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

<figure><img src="image_2.png" /></figure>

Distributed under a Creative Commons Attribution 4.0 International License
Article

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

Johan Aubray * and Florence Nicol

Ecole Nationale de l'Aviation Civile, Université de Toulouse, 7, Avenue Edouard Belin, 31400 Toulouse, France;
florence.nicol@enac.fr

* Correspondence: johan.aubray@enac.fr

**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(n)$, 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(3)$ context. Given a set of noisy points in $SE(3)$ 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.

**Keywords:** Lie groups; $SE(n)$; regression; Riemannian manifolds; affine connection; linear connection; air traffic management

## 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

<figure><img src="image_3.png" /></figure>

### check for updates

**Citation:** Aubray, J.; Nicol, F. Polynomial Regression on Lie Groups and Application to $SE(3)$. *Entropy* 2024, 26, 825. https://doi.org/10.3390/e26100825

Academic Editors: Carlo Cafaro and Antonio M. Scarfone

Received: 30 May 2024
Revised: 14 September 2024
Accepted: 24 September 2024
Published: 27 September 2024

<figure><img src="image_4.png" /></figure>

**Copyright:** © 2024 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (https://creativecommons.org/licenses/by/4.0/).
poses of a rigid object, the appropriate Lie group is the Special Euclidean group SE(n),
which has been used in the last few years in the fields of navigation [6–8]; robotics [9,10];
computational anatomy [11–14]; automation and control theory [15–17], which explains
concepts involved in calculus on manifolds such as covariant derivatives and curvature;
and signal processing [18–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 $k$th-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(3). Our goal is to follow the same idea by modeling
the motion of rigid bodies and rigid transformations with the Special Euclidean group
$SE(n)$, $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 $\{(U_i, \phi_i)\}_{i \in I}$ satisfying the following axioms:

* Each $U_i$ is an open subset of $\mathcal{M}$ and, $\mathcal{M} \subset \cup_i U_i$;
* Each $\phi_i$ is a bijection from $U_i$ into an open subset $\phi_i(U_i)$ of $\mathbb{R}^n$, and $\phi_i(U_i \cup U_j) \subset \mathbb{R}^n$ is open for every $i$ and $j$;

* For every pair $(i, j)$, the map $\phi_j \circ \phi_i^{-1}: \phi_i(U_i \cap U_j) \to \phi_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 $(E, \pi, \mathcal{M})$ with $\pi: E \to \mathcal{M}$ a smooth onto mapping such that

(i) For all $p \in \mathcal{M}, \pi^{-1}(p)$ is a real vector space isomorphic to $\mathbb{R}^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 $\phi_U: U \times \mathbb{R}^k \to \pi^{-1}(U)$ such that

* $\forall q \in U, v \in \mathbb{R}^k, \pi(\phi_U(q, v)) = q$;

* $\forall q \in U$, the mapping $v \in \mathbb{R}^k \mapsto \phi_U(q, v)$ is an isomorphism on $\phi^{-1}(q)$.

**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(s(p)) = 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(U; E)$. When $U = \mathcal{M}$, it is simply denoted by $\Gamma(E)$.

**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 $\nabla$ 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(\cdot, \cdot)$ 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^*$.
$x \mapsto s(\phi(x))$ where $s$ is a local section of $F$. Furthermore, there exists a bundle morphism $\tilde{\phi}: E \to F$ sending a generating section $s \circ \phi$ to $s$.

**Definition 12.** Let $\mathcal{M}, \mathcal{N}$ be smooth manifolds and $\phi: \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 $\nabla$ by $\phi$, designated by $\nabla^\phi$, is defined by generating sections $s \circ \phi$ as follows:

$$ \nabla_X^\phi s \circ \phi = \nabla_{D\phi X} s \qquad (8) $$

where $X \in TM$.

**Remark 6.** The quantities in Equation (8) depend only on $X(p) \in T_p\mathcal{M}$ and $T_{\phi(p)}\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 $\nabla$ 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(E)$ along $\gamma$ is the mapping:

$$ t \in ]0, 1[ \to \nabla_1^\gamma s \circ \gamma(t) \qquad (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:

$$ ds^i(\dot{\gamma}(t))e_i + \Gamma_{ij}^k s^i \dot{\gamma}^j(t)e_n \qquad (10) $$

**Proof.** This result is a direct application of Definition 12 with $\phi = \gamma$. $\square$

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

$$ \frac{d}{dt} s^i(t) e_i \qquad (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(E)$ is said to be $\nabla$-parallel along $\gamma$ if

$$ \nabla_{\dot{\gamma}} s = 0. $$

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

$$ \frac{d}{dt} s^k(t) + \Gamma_{ij}^k s^i \dot{\gamma}^j(t) = 0, \quad k = 1 \dots n \qquad (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}, TM, \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}{l} \dot{\gamma}(t) = s \circ \gamma(t) \\ \nabla_{\gamma} s = 0 \\ \gamma(0) = x \end{array} \right. \qquad (13) $$

**Definition 15.** Let $(\mathcal{M}, TM, \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 $\gamma$ 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(1) \qquad (14) $$

where $\gamma$ 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) \qquad (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:

$$ \frac{d}{dt} \exp_x tX = s(\gamma(t)) \qquad (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 $\gamma$:

$$ \nabla_{\dot{\gamma}} s = 0. \qquad (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( (\partial_j Y^i) X^j - (\partial_j X^i) Y^j \right) \partial_i = 0 $$

<figure><img src="image_5.png" /></figure>

## 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^{\sharp}$ is the only element of $TM$ such that, for all $X \in TM$,

$$ \alpha(X) = g(\alpha^{\sharp}, X) \qquad (18) $$

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

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

$$ Y^{\flat}(X) = g(X, Y) \qquad (19) $$

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

Both isomorphisms can be expressed in local coordinates:

$$ \alpha^{\sharp} = \sharp(\alpha) = \sharp(\alpha_i e^i) = \underbrace{g^{ij} \alpha_j e_i}_{X^i}, $$

$$ X^{\flat} = \flat(X) = \flat(X^i e_i) = \underbrace{g_{ij} X^j e^i}_{\alpha_i} $$

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

$$ \frac{\partial^{|I|} 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 $|I| = \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 $df_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(0) = \gamma_2(0) = p$ and $(U, \phi)$ a local chart such that $\forall i \in 1 \dots k$,

$$ \left. \frac{d^i}{dt^i} \right|_{t=0} \phi \circ \gamma_1(t) = \left. \frac{d^i}{dt^i} \right|_{t=0} \phi \circ \gamma_2(t) $$

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}, [\gamma]_p^m \mapsto [\gamma]_p^{m-1} $$

are projections from one level of the bundle to the one under.
<figure><img src="image_6.png" /></figure>

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: $df_p = 0$.

**Proof.** See [34] □

In fact, if $\gamma: ] -\epsilon, +\epsilon[ \to M$ is a smooth curve on $M$ such that $\gamma(0) = p$, then $df_p(\dot{\gamma}(0))$ is the first-order variation of $f$ in the direction of the tangent vector $\dot{\gamma}(0) \in T_pM$. 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: TM \to \mathcal{M}, (x, v) \mapsto R_x(v)$$

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

**Remark 10.** The Riemannian exponential $(x, v) \mapsto \exp_x(v)$ 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]:

<caption>**Algorithm 1: Riemannian gradient descent**</caption>

**Input:** $x_0 \in \mathcal{M}$
1. **for** $k=0, 1, 2, \dots$ **do**
2. &nbsp;&nbsp;&nbsp;&nbsp;Pick a step size $\alpha_k > 0$
3. &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;$x_{k+1} = R_{x_k}(-\alpha_k \text{grad} f(x_k))$

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}^{\dim G}$:

$$ \langle\langle X_g, Y_g \rangle\rangle_g = \langle T_g L_{g^{-1}} X_g, T_g L_{g^{-1}} Y_g \rangle \qquad (20) $$

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

$$ \langle\langle X_g, Y_g \rangle\rangle_{A, g} = \langle T_g L_{g^{-1}} A X_g, T_g L_{g^{-1}} A Y_g \rangle \qquad (21) $$

Likewise, a right-invariant metric can also be defined. A minimizing geodesic between two points $g_0, g_1$ with respect to $\langle\langle \cdot, \cdot \rangle\rangle_g$ is a curve $\gamma: [0, 1] \to G$, such that $\gamma(0) = g_0$, $\gamma(1) = g_1$ in minimizing the functional

$$ L(\gamma) = \frac{1}{2} \int_{0}^{1} \langle\langle \dot{\gamma}(t), \dot{\gamma}(t) \rangle\rangle_{g} dt \qquad (22) $$

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

$$ L(X) = \frac{1}{2} \int_{0}^{1} \langle\langle \dot{X}(t), \dot{X}(t) \rangle\rangle_{e} dt \qquad (23) $$

where $X: ] - \epsilon, \epsilon[ \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}(t) = ad_{X(t)}^{t} X(t) \qquad (24) $$

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

$$ \dot{\gamma}(t) = T_e L_{\gamma(t)} X(t) \qquad (25) $$

which simplifies in a matrix Lie group as follows:

$$ \gamma(t) = \exp\left(\int_{0}^{t} X(u)du\right) \qquad (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 $\nabla$ is a connection on $G$, then a minimizing geodesic $\gamma$ has to satisfy the following:

$$ \nabla_{\dot{\gamma}(t)} \dot{\gamma}(t) = 0 \qquad (27) $$

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

$$ \left(\nabla_{\dot{\gamma}(t)}\right)^k \dot{\gamma}(t) = 0 \qquad (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 $\bar{\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} = \frac{1}{2} (c_{ij}^{k} + c_{ki}^{j} - c_{jk}^{i}) \qquad (29) $$

where the $c_{ij}^k = \langle [e_i, e_j], e_k \rangle$ 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 $g$ by solving an augmented differential equation

$$v_1(t) = \nabla_{X(t)}v_0(t), v_2(t) = \nabla_{X(t)}v_1(t), \dots, \nabla_{X(t)}v_{p-1}(t) = 0 \quad (30)$$

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

$$\frac{d}{dt} \begin{pmatrix} v_0 \\ \vdots \\ v_{p-1} \end{pmatrix} = F(v_0, \dots, v_{p-1}) \quad (31)$$

where the vector $v_0, \dots, v_{p-1}$ is in $\mathbb{R}^{pn}$, with $n$ the dimension of $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 $(M, g)$. The idea is to find a geodesic curve $\gamma(t)$ 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(t)$ and the data.

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

$$Y = \operatorname{Exp}_{\gamma(t)}(\varepsilon) = \operatorname{Exp}_{\operatorname{Exp}_p(tv)}(\varepsilon)$$

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

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

$$Y = \operatorname{Exp}_{\gamma(t)}(\varepsilon)$$

where the curve $\gamma(t)$ 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(0), \dot{\gamma}(0), \dots, \dot{\gamma}^{(i)}(0), \dots, \dot{\gamma}^{(k)}(0))$ as follows:

$$E(\gamma) = \frac{1}{N} \sum_{j=1}^{N} d(\gamma(t_j), y_j)^2 \quad (32)$$

where $d$ is a geodesic distance between the model $\gamma(t_j)$ 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}(t)}\dot{\gamma}(t) = 0$, or, more generally, a $k$-th order polynomial constraint $(\nabla_{\dot{\gamma}(t)})^k\dot{\gamma}(t) = 0$ that is uniquely solved by adding initial conditions $\gamma(0), \dot{\gamma}(0), \dots, \dot{\gamma}^{(i)}(0), \dots, \dot{\gamma}^{(k)}(0)$.

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 $g$, we obtain the "forward" polynomial equations

$$\left\{
\begin{aligned}
\frac{d}{dt}\gamma(t) &= \gamma(t)X_1(t) \\
\vdots \\
\frac{d}{dt}X_i(t) &= \bar{\nabla}_{X_1}X_i(t) + X_{i+1}(t) \\
\vdots \\
\frac{d}{dt}X_k(t) &= \bar{\nabla}_{X_1}X_k(t)
\end{aligned}
\right.
\quad (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 $\lambda$, 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 $\Phi$ with respect to $\lambda$ is the solution of the following:

$$ \frac{d}{dt} \partial_{\lambda} \Phi_{X}(t, x; \lambda) = \partial_{x} X(\Phi(t, x; \lambda); \lambda) \partial_{\lambda} \Phi_{X}(t, x; \lambda) + \partial_{\lambda} X(\Phi_{X}(t, x; \lambda), \lambda) \quad (34) $$

with the initial condition as follows:

$$ \partial_{\lambda} \Phi_{X}(0, x; \lambda) = 0. \quad (35) $$

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

$$ \frac{d}{dt} \partial_x \Phi_X(t, x; \lambda) = \partial_x X(\Phi(t, x; \lambda); \lambda) \partial_x \Phi_X(t, x; \lambda) \quad (36) $$

with the initial condition as follows:

$$ \partial_x \Phi_X(0, x; \lambda) = \mathrm{Id}. \quad (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}(0) + \lambda v$ while keeping the starting point $p = \gamma(0)$ constant. The one-parameter family of curves will be denoted by $\gamma(p, t; \lambda)$. Letting $J = \partial_{\lambda}\gamma$, we obtain the following:

$$ \frac{d^2}{dt^2} J^i = - \left( \partial_l \Gamma^i_{jk} \right) \dot{\gamma}^i \dot{\gamma}^j J^l - 2 \Gamma^i_{jk} \frac{d}{dt} J^j \dot{\gamma}^k, \quad (38) $$

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

$$ \frac{d^2}{dt^2} J + R^{\nabla}(J, \dot{\gamma})\dot{\gamma} = 0. \quad (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 $\gamma$ 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(0) = p$, an application of Lemma 2 and Proposition 7 yields the next expression for the differential at $T_p M$ of $E(\gamma)$ with respect to the initial conditions as follows:
$$dE: Y \in T_p M \mapsto \left\{ \begin{array}{l} \frac{2}{N} \sum_{i=1}^{N} g\left(\gamma(t_i); \partial_p \gamma(t_i, p).Y, \log_{\gamma(t_i, p)} y_i\right), \\ \frac{2}{N} \sum_{i=1}^{N} g\left(\gamma(t_i); \partial_v \gamma(t_i, p).Y, \log_{\gamma(t_i, p)} y_i\right), \\ v = \dot{\gamma}(0). \end{array} \right. \quad (40)$$

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

$$\forall Y \in T_p M, \left\{ \begin{array}{l} g(p; Y, Z_0) = \frac{2}{N} \sum_{i=1}^{N} g\left(\gamma(t_i); \partial_p \gamma(t_i, p).Y, \log_{\gamma(t_i, p)} y_i\right), \\ g(p; Y, Z_1) = \frac{2}{N} \sum_{i=1}^{N} g\left(\gamma(t_i); \partial_v \gamma(t_i, p).Y, \log_{\gamma(t_i, p)} y_i\right). \end{array} \right. \quad (41)$$

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

$$dE_i^p := g\left(\gamma(t_i); \partial_p \gamma(t_i, p).Y, \log_{\gamma(t_i, p)} y_i\right), \quad (42)$$

$$dE_i^v := g\left(\gamma(t_i); \partial_v \gamma(t_i, p).Y, \log_{\gamma(t_i, p)} y_i\right), \quad (43)$$

$\partial_* \gamma(t_i, p).Y$ and $\log_{\gamma(t_i, p)} y_i$ both belong to $T_{\gamma(t_i)} M$. As illustrated in Figure 1 (where $\Pi_{t_k}^0$ denotes the parallel transport from $T_{\gamma(t_k)} M$ to $T_{\gamma(0)} M$, and $\epsilon_k = \log_{\gamma(t_k, p)} y_k$ denotes the residue at time $t_k$), we can transport them back to $T_p M$, so that there are two vectors $(Z_0^i, Z_1^i)$ such that

$$dE_i^p = g\left(\gamma(t_i); \partial_p \gamma(t_i, p).Y, \epsilon_i\right) = g(p; Y, Z_0^i), \quad (44)$$

$$dE_i^v = g\left(\gamma(t_i); \partial_v \gamma(t_i, p).Y, \epsilon_i\right) = g(p; Y, Z_1^i). \quad (45)$$

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

$$\forall Y \in T_p M, \left\{ \begin{array}{l} g(p; Y, Z_0) = \frac{2}{N} \sum_{i=1}^{N} g(p; Y, Z_0^i) = \frac{2}{N} g(p; Y, \sum_{i=1}^{N} Z_0^i), \\ g(p; Y, Z_1) = \frac{2}{N} \sum_{i=1}^{N} g(p; Y, Z_1^i) = \frac{2}{N} g(p; Y, \sum_{i=1}^{N} Z_1^i). \end{array} \right. \quad (46)$$

<figure><img src="image_7.png" /><figcaption>Figure 1. Parallel transport of the residues.</figcaption></figure>

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(t_i)} M$ back to $T_p 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 deriva-
tion of the adjoint equations can be performed mutantis mutandis using the dual-connection $\nabla^*$ as follows:

$$ \nabla_{\hat{\gamma}}^{*} \lambda_{i}(t) = -\lambda_{i-1}(t), \qquad (47) $$

$$ \nabla_{\hat{\gamma}}^{*} \lambda_{0}(t) = - \sum_{i} R^{\nabla^{*}}(v_{i}(t), \lambda_{i}(t)) v_{1}(t), \qquad (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(3), we remind here some specific elements about SE(n) and its Lie algebra se(n).

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

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

$$ \begin{pmatrix} R_1 & T_1 \\ 0 & 1 \end{pmatrix} \times \begin{pmatrix} R_2 & T_2 \\ 0 & 1 \end{pmatrix} = \begin{pmatrix} R_1 R_2 & R_1 T_2 + T_1 \\ 0 & 1 \end{pmatrix} $$

and the inverse of an element is as follows:

$$ \begin{pmatrix} R & T \\ 0 & 1 \end{pmatrix}^{-1} = \begin{pmatrix} R^T & -R^T T \\ 0 & 1 \end{pmatrix} $$

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

**Definition 20.** The vector space of real (n + 1)-by-(n + 1) matrices of the form

$$ X = \begin{pmatrix} K & V \\ 0 & 0 \end{pmatrix} \qquad (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(n).

**Remark 13.** It can be easily shown that the matrix exponential of any matrix defined by (49) belongs to SE(n). It can also be proven by the exp map se(n) → SE(n) 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π.

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

$$ E_1 = \begin{pmatrix} 0 & 0 & 0 & 0 \\ 0 & 0 & -1 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{pmatrix}, E_2 = \begin{pmatrix} 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 \\ -1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{pmatrix}, E_3 = \begin{pmatrix} 0 & -1 & 0 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{pmatrix}, $$

$$ E_4 = \begin{pmatrix} 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{pmatrix}, E_5 = \begin{pmatrix} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{pmatrix}, E_6 = \begin{pmatrix} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \end{pmatrix} $$
There is a canonical isomorphism between $\mathfrak{sc}(3)$ and $\mathbb{R}^6$ often denoted $\wedge$ (“hat”), with its inverse being denoted $\vee$ (“vee”) as follows:

$$ \wedge: \mathbb{R}^6 \to \mathfrak{sc}(3), \vee: \mathfrak{sc}(3) \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(3)$ that have a bi-invariant metric, but no such closed form can be derived for a higher-order polynomial. Unfortunately, $SE(3)$ 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{sc}(3)$ 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:

$$ \langle A, B \rangle = tr(A^T B) $$

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(0)$ 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.

$$ \left\{ \begin{array}{l} \frac{d}{dt} \gamma(t) = \gamma(t) X_1(t) \\ \vdots \\ \frac{d}{dt} X_i(t) = \bar{\nabla}_{X_1} X_i(t) + X_{i+1}(t) \\ \vdots \\ \frac{d}{dt} X_k(t) = \bar{\nabla}_{X_1} X_k(t) \end{array} \right. \implies \left\{ \begin{array}{l} \gamma(t) = \exp\left(\int_0^t X_1(u) du\right) \\ \vdots \\ \dot{X}_i(t) = \Gamma_{ij}^m X_1^i X_i^j + X_{i+1} \\ \vdots \\ \dot{X}_k(t) = \Gamma_{ij}^m X_1^i X_k^j \end{array} \right. $$
<caption>**Algorithm 2:** Simulated data generation</caption>

1 **for** *r* = 1 **to** *Nman* **do**
   Choose: $v_{\text{sc3}}^r$ /* velocity in the Lie algebra, corresponding to the
   $r^{\text{th}}$ maneuver, assumed to be constant */
   2 &nbsp;&nbsp;&nbsp; $\xi \leftarrow v_{\text{sc3}}^r$
   3 **for** *i* = 1 **to** *Nmeas* **do**
   4 &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; $y_{\text{real}}^{\text{SE3}}(\frac{i}{N_{\text{meas}}}) = \exp(\frac{i}{N_{\text{meas}}}\xi)$ /* $\exp(t\xi) \in G$ with $t = \frac{i}{N}$ */
   5 &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; $y_{\text{real}}^{R3}(\frac{i}{N_{\text{meas}}}) = y_{\text{real}}^{\text{SE3}}(\frac{i}{N_{\text{meas}}}) \times x_0$
   6 &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; $y_{\text{meas}}^{\text{SE3}}(\frac{i}{N_{\text{meas}}}) = \exp(\frac{i}{N_{\text{meas}}}\xi + n(\frac{i}{N_{\text{meas}}}))$ /* $n \sim \mathcal{N}(0, \sigma^2) \in \text{sc}(3)$ */
   7 &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; $y_{\text{meas}}^{R3}(\frac{i}{N_{\text{meas}}}) = y_{\text{meas}}^{\text{SE3}}(\frac{i}{N_{\text{meas}}}) \times x_0$
   8 &nbsp;&nbsp;&nbsp; $x_0 \leftarrow y_{\text{meas}}^{R3}(1)$ /* Update of the starting point before the next maneuver */

**Output:**

* y\_meas\_SE3, noisy trajectory as a set of motions in SE(3),

• y\_real\_SE3, real trajectory as a set of motions in SE(3),

• y\_meas\_R3, noisy trajectory as a set of points in R³,

• y\_real\_R3, real trajectory as a set of points in R³.

<caption>**Algorithm 3:** Determining the optimum geodesic</caption>

1 **while** ||Z₀|| > δmin **OR** ||Z₁|| > δmin **do**
2 &nbsp;&nbsp;&nbsp; **Solve in sc(3):** /* Forward equations with the Christoffel symbols */
3 &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; ˙γₑ(t) = γₑ(t)v(t)
4 &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; ˙v(t) + Γᵢⱼ^{m}vᵢvⱼ = 0
5 &nbsp;&nbsp;&nbsp; **Solve in SE(3):**
6 &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; γ(t) = expₚ(γₑ(t))
7 &nbsp;&nbsp;&nbsp; **Solve the backward Equations (44) and (45):**
8 &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; **for** *j* = 1:Nmeas **do**
9 &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; Compute to *t* = 0 each *dE*ᵢⱼ^{p}, *dE*ᵢⱼ^{v} by parallel transport of the residue *ε*ᵢ
&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; translated back to sc(3)
10 &nbsp;&nbsp;&nbsp; Compute Z₀, Z₁ ∈ sc(3) /* Equations (46) */
11 &nbsp;&nbsp;&nbsp; **Update of p and v:**
12 &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; v ← v + εZ₁ /* displacement on sc(3) */
13 &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; *p* ← expₚ(εZ₀) /* displacement on SE(3) */
&nbsp;&nbsp;&nbsp; **Optional:** Change ε for better convergence
The first examples are shown in Figures 2 and 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(3) being close to zero but not exactly zero. This could be corrected by post-processing by the flagging of the trajectory.

<figure><img src="image_8.png" /><figcaption>Figure 2. A straight line.</figcaption></figure>

<figure><img src="image_9.png" /><figcaption>Figure 3. A straight line, with a "large" additive noise.</figcaption></figure>

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 Figures 5 and 6.
<figure><img src="image_10.png" /><figcaption>Figure 4. A helix.</figcaption></figure>

We can measure how well the model fits the data using a coefficient of determination R². 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 = \frac{1}{N} \min_{y \in \mathcal{M}} \sum_j d(y, y_j)^2, $$

leading to

$$ R^2 = 1 - \frac{\text{SSE}}{\text{sample Fréchet variance}} = 1 - \frac{\sum_j d(\gamma(t_j), y_j)^2}{\min_{y \in \mathcal{M}} \sum_j d(y, y_j)^2} \quad (50) $$
Note that since we use simulated data, we can also measure the R² coefficient with respect to the “real” data:

$$R^2 = 1 - \frac{\sum_j d(\gamma(t_j), y_j^{\text{real}})^2}{\min_{y \in \mathcal{M}} \sum_j d(y, y_j^{\text{real}})^2} \quad (51)$$

Similarly to the Euclidean case, an R² 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² 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.

<figure><img src="image_11.png" /><figcaption>Figure 5. A gradual turn.</figcaption></figure>

<figure><img src="image_12.png" /><figcaption>Figure 6. A sharp turn.</figcaption></figure>
<table><caption>**Table 1.** Results after simulations.</caption><thead><tr><th>Maneuver</th><th>Noise Std</th><th>Nmeas</th><th>R<sup>2</sup></th></tr></thead><tbody><tr><td>Straight line</td><td>10<sup>-3</sup></td><td>25</td><td>1</td></tr><tr><td><strong>Straight line</strong></td><td>10<sup>-2</sup></td><td><strong>25</strong></td><td><strong>0.99</strong></td></tr><tr><td>Straight line</td><td>10<sup>-1</sup></td><td>25</td><td>0.84</td></tr><tr><td>Straight line</td><td>10<sup>-2</sup></td><td>25</td><td>0.99</td></tr><tr><td>Straight line</td><td>10<sup>-2</sup></td><td>15</td><td>0.99</td></tr><tr><td>Straight line</td><td>10<sup>-2</sup></td><td>10</td><td>0.99</td></tr><tr><td>Straight line</td><td>10<sup>-1</sup></td><td>25</td><td>0.84</td></tr><tr><td>Straight line</td><td>10<sup>-1</sup></td><td>15</td><td>0.82</td></tr><tr><td>Straight line</td><td>10<sup>-1</sup></td><td>10</td><td>0.81</td></tr><tr><td>Gradual turn</td><td>10<sup>-2</sup></td><td>25</td><td>0.99</td></tr><tr><td><strong>Gradual turn</strong></td><td>10<sup>-2</sup></td><td><strong>15</strong></td><td><strong>0.99</strong></td></tr><tr><td>Gradual turn</td><td>10<sup>-2</sup></td><td>10</td><td>0.99</td></tr><tr><td>Gradual turn</td><td>10<sup>-1</sup></td><td>25</td><td>0.79</td></tr><tr><td>Gradual turn</td><td>10<sup>-1</sup></td><td>15</td><td>0.75</td></tr><tr><td>Gradual turn</td><td>10<sup>-1</sup></td><td>10</td><td>0.74</td></tr><tr><td><strong>Sharp turn</strong></td><td>10<sup>-1</sup></td><td><strong>25</strong></td><td><strong>0.92</strong></td></tr><tr><td>Sharp turn</td><td>10<sup>-1</sup></td><td>15</td><td>0.89</td></tr><tr><td>Sharp turn</td><td>10<sup>-1</sup></td><td>10</td><td>0.81</td></tr></tbody></table>

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(3) 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.

**Supplementary Materials:** All MATLAB scripts and some datasets are freely available at https://github.com/jhnaby/LieGroupRegression (accessed on 26 September 2024).

**Author Contributions:** Conceptualization, J.A. and F.N.; software, J.A. and F.N.; investigation, F.N., writing—original draft preparation, J.A.; writing—review and editing, J.A. and F.N. All authors have read and agreed to the published version of the manuscript.

**Funding:** The APC was funded by Ecole Nationale de l'Aviation Civile.

**Institutional Review Board Statement:** Not applicable.

**Data Availability Statement:** The original data presented in the study are openly available in GitHub at https://github.com/jhnaby/LieGroupRegression (accessed on 26 September 2024).
**Conflicts of Interest:** The authors declare no conflicts of interest.

**References**

1. Julier, S.J.; Uhlmann, J.K. New extension of the Kalman filter to nonlinear systems. In Proceedings of the Signal Processing, Sensor Fusion, and Target Recognition VI, Orlando, FL, USA, 21–24 April 1997; Kadar, I., Ed.; International Society for Optics and Photonics; SPIE: Bellingham, DC, USA, 1997; Volume 3068, pp. 182–193. [CrossRef]

2. Bourmaud, G.; Mégret, R.; Giremus, A.; Berthoumieu, Y. Discrete Extended Kalman Filter on Lie groups. In Proceedings of the 21st European Signal Processing Conference (EUSIPCO 2013), Marrakech, Morocco, 9–13 September 2013; pp. 1–5.

3. Phogat, K.S.; Chang, D.E. Invariant extended Kalman filter on matrix Lie groups. *Automatica* **2020**, *114*, 108812. [CrossRef]

4. Bonnabel, S. Left-invariant Extended Kalman Filter and attitude estimation. In Proceedings of the 2007 46th IEEE Conference on Decision and Control, New Orleans, LA, USA, 12–14 December 2007; pp. 1027–1032. [CrossRef]

5. Bonnabel, S.; Martin, P.; Salaün, E. Invariant Extended Kalman Filter: Theory and application to a velocity-aided attitude estimation problem. In Proceedings of the 48h IEEE Conference on Decision and Control (CDC) Held Jointly with 2009 28th Chinese Control Conference, Shanghai, China, 15–18 December 2009; pp. 1297–1304. [CrossRef]

6. Fang, K.; Cai, T.; Wang, B. The Kinematic Models of the SINS and Its Errors on the SE(3) Group in the Earth-Centered Inertial Coordinate System. *Sensors* **2024**, *24*, 3864. [CrossRef] [PubMed]

7. Jeong, D.B.; Lee, B.; Ko, N.Y. Three-Dimensional Dead-Reckoning Based on Lie Theory for Overcoming Approximation Errors. *Appl. Sci.* **2024**, *14*, 5343. [CrossRef]

8. Sun, J.; Chen, Y.; Cui, B. An Improved Initial Alignment Method Based on SE2(3)/EKF for SINS/GNSS Integrated Navigation System with Large Misalignment Angles. *Sensors* **2024**, *24*, 2945. [CrossRef] [PubMed]

9. Park, F.; Bobrow, J.; Ploen, S. A Lie Group Formulation of Robot Dynamics. *Int. J. Robot. Res.* **1995**, *14*, 609–618. [CrossRef]

10. Wang, W.; Peng, X.; Ai, J.; Fu, C.; Li, C.; Zhang, Z. SE(3) Based LTV-MPC Algorithm for Multi-Obstacle Trajectory Tracking of Fully Driven Spacecraft. *IEEE Access* **2024**, *12*, 37850–37861. [CrossRef]

11. Miolane, N.; Pennec, X. Computing Bi-Invariant Pseudo-Metrics on Lie Groups for Consistent Statistics. *Entropy* **2015**, *17*, 1850–1881. [CrossRef]

12. Boisvert, J.; Pennec, X.; Ayache, N.; Labelle, H.; Cheriet, K. 3D anatomical variability assessment of the scoliotic spine using statistics on Lie groups. In Proceedings of the 3rd IEEE International Symposium on Biomedical Imaging: Nano to Macro, Arlington, VA, USA, 6–9 April 2006; pp. 750–753. [CrossRef]

13. Boisvert, J.; Cheriet, F.; Pennec, X.; Labelle, H.; Ayache, N. Geometric Variability of the Scoliotic Spine Using Statistics on Articulated Shape Models. *IEEE Trans. Med Imaging* **2008**, *27*, 557–568. [CrossRef] [PubMed]

14. Hanik, M.; Hege, H.C.; von Tycowicz, C. Bi-Invariant Dissimilarity Measures for Sample Distributions in Lie Groups. *Siam J. Math. Data Sci.* **2022**, *4*, 1223–1249. [CrossRef]

15. Fiori, S.; Rossi, L.D. Minimal control effort and time Lie-group synchronisation design based on proportional-derivative control. *Int. J. Control* **2022**, *95*, 138–150. [CrossRef]

16. Duan, X.; Sun, H.; Zhao, X. A Matrix Information-Geometric Method for Change-Point Detection of Rigid Body Motion. *Entropy* **2019**, *21*, 531. [CrossRef] [PubMed]

17. Fiori, S. Manifold Calculus in System Theory and Control—Second Order Structures and Systems. *Symmetry* **2022**, *14*, 1144. [CrossRef]

18. Smith, S. Covariance, subspace, and intrinsic Cramer-Rao bounds. *IEEE Trans. Signal Process.* **2005**, *53*, 1610–1630. [CrossRef]

19. Labsir, S.; Renaux, A.; Vilà-Valls, J.; Chaumette, É. Cramér-Rao Bound on Lie Groups with Observations on Lie Groups: Application to SE(2). In Proceedings of the ICASSP 2023—2023 IEEE International Conference on Acoustics, Speech and Signal Processing (ICASSP), Rhodes Island, Greece, 4–10 June 2023; pp. 1–5. [CrossRef]

20. Labsir, S.; Giremus, A.; Yver, B.; Benoudiba-Campanini, T. An intrinsic Bayesian bound for estimators on the Lie groups SO(3) and SE(3). *Signal Process.* **2024**, *214*, 109232. [CrossRef]

21. Jeon, J.M.; Park, B.U.; Keilegom, I.V. Nonparametric regression on Lie groups with measurement errors. *Ann. Stat.* **2022**, *50*, 2973–3008. [CrossRef]

22. Camarinha, M.; Silva Leite, F.; Crouch, P. On the geometry of Riemannian cubic polynomials. *Differ. Geom. Its Appl.* **2001**, *15*, 107–135. [CrossRef]

23. Camarinha, M.; Silva Leite, F.; Crouch, P. High-Order Splines on Riemannian Manifolds. *Proc. Steklov Inst. Math.* **2023**, *321*, 158–178. [CrossRef]

24. Hinkle, J.; Muralidharan, P.; Fletcher, P.T.; Joshi, S. Polynomial Regression on Riemannian Manifolds. In Proceedings of the Computer Vision—ECCV 2012, Florence, Italy, 7–13 October 2012; Fitzgibbon, A., Lazebnik, S., Perona, P., Sato, Y., Schmid, C., Eds.; Springer: Berlin/Heidelberg, Germany, 2012; pp. 1–14.

25. Popiel, T.; Noakes, L. Bézier curves and C2 interpolation in Riemannian manifolds. *J. Approx. Theory* **2007**, *148*, 111–127. [CrossRef]

26. Hanik, M.; Hege, H.C.; Hennemuth, A.; von Tycowicz, C. Nonlinear Regression on Manifolds for Shape Analysis using Intrinsic Bézier Splines. In Proceedings of the Medical Image Computing and Computer Assisted Intervention—MICCAI 2020, Lima, Peru, 4–8 October 2020; Martel, A.L., Abolmaesumi, P., Stoyanov, D., Mateus, D., Zuluaga, M.A., Zhou, S.K., Racoceanu, D., Joskowicz, L., Eds.; Springer: Cham, Switzerland, 2020; pp. 617–626.
27. Fletcher, P.T. Geodesic Regression on Riemannian Manifolds. In Proceedings of the Third International Workshop on Mathematical Foundations of Computational Anatomy—Geometrical and Statistical Methods for Modelling Biological Shape Variability, Westin Harbour Castle, TO, Canada, 22 September 2011; Pennec, X., Joshi, S., Nielsen, M., Eds.; pp. 75–86.

28. Pennec, X. Bi-invariant Means on Lie Groups with Cartan-Schouten Connections. In Proceedings of the Geometric Science of Information, Paris, France, 28–30 August 2013; Nielsen, F., Barbaresco, F., Eds.; Springer: Berlin/Heidelberg, Germany, 2013; pp. 59–67.

29. Amari, S.I.; Nagaoka, H. *Methods of Information Geometry*; Translations of Mathematical Monographs 191; American Mathematical Society and Oxford University Press: Oxford, UK, 2000.

30. Husemöller, D. *Fibre Bundles*; Graduate Texts in Mathematics; Springer: New York, NY, USA, 2013.

31. Willmore, T. *Riemannian Geometry*; Oxford Science Publications; Clarendon Press: Oxford, UK, 1996.

32. Agrachev, A.A.; Sachkov, Y.L. *Control Theory from the Geometric Viewpoint*; Encyclopaedia of Mathematical Sciences; Springer: Berlin/Heidelberg, Germany, 2004. [CrossRef]

33. Saunders, D.J. *The Geometry of Jet Bundles*; London Mathematical Society Lecture Note Series; Cambridge University Press: Cambridge, UK, 1989.

34. Boumal, N. *An Introduction to Optimization on Smooth Manifolds*; Cambridge University Press: Cambridge, UK, 2023. [CrossRef]

35. Marsden, J.; Ratiu, T. *Introduction to Mechanics and Symmetry: A Basic Exposition of Classical Mechanical Systems*; Texts in Applied Mathematics; Springer: New York, NY, USA, 1999.

36. Niethammer, M.; Huang, Y.; Vialard, F.X. Geodesic Regression on Image Time Series. In Proceedings of the Medical Image Computing and Computer-Assisted Intervention: MICCAI—International Conference on Medical Image Computing and Computer-Assisted Intervention, Toronto, ON, Canada, 18–22 September 2011; Volume 14, pp. 655–662. [CrossRef]

37. Duistermaat, J.; Kolk, J. *Lie Groups*; Universitext; Springer: Berlin/Heidelberg, Germany, 1999.

38. Fréchet, M. Les éléments aléatoires de nature quelconque dans un espace distancié. *Ann. L'Institut Henri Poincaré* **1948**, *10*, 215–310.

**Disclaimer/Publisher's Note:** The statements, opinions and data contained in all publications are solely those of the individual author(s) and contributor(s) and not of MDPI and/or the editor(s). MDPI and/or the editor(s) disclaim responsibility for any injury to people or property resulting from any ideas, methods, instructions or products referred to in the content.