# An Optimal Quantization-based Unscented Kalman Filter on SE(2): Application to Trajectory Tracking

Vivien Pravong, Jean-Philippe Condomines, Gustav Öman Lundin, Stéphane Puechmorel

► To cite this version:

Vivien Pravong, Jean-Philippe Condomines, Gustav Öman Lundin, Stéphane Puechmorel. An Optimal Quantization-based Unscented Kalman Filter on SE(2): Application to Trajectory Tracking. 2025 European Control Conference (ECC), Jun 2025, Thessaloniki, Greece. IEEE, pp.2012-2018, 2025, 978-3-907144-12-1. 10.23919/ECC65951.2025.11187249. hal-05029097

HAL Id: hal-05029097

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

Submitted on 10 Apr 2025

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.

Public Domain
An Optimal Quantization-based Unscented Kalman Filter on SE(2):
Application to Trajectory Tracking

Vivien Pravong<sup>1,3</sup>, Jean-Philippe Condomines<sup>2,3</sup>, Gustav Öman Lundin<sup>1,3</sup>, and Stéphane Puechmorel<sup>2</sup>

<sup>1</sup>DTIS, ONERA, Université de Toulouse, 31000, Toulouse, France

firstname.lastname@onera.fr

<sup>2</sup>Ecole Nationale de l'Aviation Civile (ENAC), Université de Toulouse, 31055 Toulouse, France

firstname.lastname@enac.fr

<sup>3</sup>Fédération ONERA ISAE-SUPAERO ENAC Université de Toulouse, France

*Abstract*— This paper introduces an Unscented Kalman Filter (UKF) on the Special Euclidean group SE(2) based on optimal quantization, integrating it within the Invariant Linear Quadratic Gaussian (ILQG) regulator to solve trajectory tracking featuring high-angle variations. Traditional trajectory tracking methods like the Linear Quadratic Gaussian (LQG) regulator often face limitations due to linearization, especially under highly nonlinear model, high-angle maneuvers and/or stochastic uncertainties. The proposed approach overcomes these limitations by employing a sigma-point filter, the Unscented Kalman Filter on Lie groups (UKF-LG) enhanced with optimal quantization. This method improves the accuracy of the covariance computation during the filter's prediction step, enabling better estimation performances to provide robust trajectory tracking in scenarios with significant angular deviations. Simulations demonstrate the effectiveness of the proposed Optimal Quantization-based Left-UKF-LG (OQ-Left-UKF-LG) over the IEKF and standard UKF-LG in complex tracking scenarios. The approach offers promising potential for applications requiring precise tracking in dynamic environments, such as autonomous mobile robots or Unmanned Aerial Vehicles (UAVs).

# I. INTRODUCTION

Mobile robot trajectory tracking, particularly for ground mobile robots and/or Unmanned Aerial Vehicles (UAVs), has advanced significantly with the development of robust control algorithms. Trajectory tracking consists of autonomously following a predefined trajectory in order to solve numerous problems such as delivery, search and rescue, or inspection operations [1], [2]. Both types of robots share the challenge of trajectory tracking under uncertainties, and modern solutions often leverage advanced control approaches to optimize real-time responses and enable safe and reliable execution of agile maneuvers across varying environments.

One of the control strategies that has been extensively studied in the literature is the Linear Quadratic Gaussian (LQG) regulator. It combines two classical algorithms: the Linear Quadratic Regulator (LQR), which minimizes a quadratic cost function of the states and inputs of the system, and the Extended Kalman Filter (EKF), which estimates the state in the presence of Gaussian noise. This fusion results in a sub-optimal observer-controller for nonlinear systems with stochastic disturbances and partial state observability. When designing an LQG regulator for a nonlinear system, a

well-known issue is the linearization of the model. When linearization is performed, the model is approximated by a Taylor series expansion truncated to the first-order term, neglecting higher-order components. This simplification reduces the model's complexity but comes at the cost of accuracy, potentially making the control objective less effective.

In response, various methods have aimed to integrate symmetry considerations into control design, resulting in models that preserve invariance under transformations. In simple terms, a dynamical system exhibits symmetries when its behavior remains unchanged under certain transformations. The control problem for dynamical systems has been formulated directly on matrix Lie groups to fully exploit the symmetry properties of Lie group theory and to represent state variables as matrices instead of vectors [3]. Similarly for the state estimation problem, there has been an increasing number of attempts to account for the symmetries in observer design [4]–[6]. The formulation of the estimation problem on matrix Lie groups for robotic systems has been considered within the Invariant Extended Kalman Filter (IEKF) framework [7]. The IEKF has been applied to the control problem in [8], where an Invariant LQG (ILQG) controller is derived for state estimation and control of a 2D non-holonomic car model. It has been shown that the ILQG regulator outperforms the classical LQG regulator by achieving a lower quadratic cost and exhibiting greater robustness against trajectory losses. However, the model presented in this paper remains expressed in a vector form and still relies on a first-order linearization of the dynamics, even though it inherently involves the Lie group SE(2). Taking inspiration from [8], an Invariant LQR expressed on SE<sub>2</sub>(3) has been developed and applied to a 3D quadrotor model in [9].

Apart from the Extended Kalman Filter, other solutions exist for solving the state estimation problem of a stochastic dynamical system, such as the Unscented Kalman Filter (UKF). It has been shown that the UKF achieves higher accuracy than the EKF, as it does not truncate the Taylor series expansion at an arbitrary order. Similar to the development of the invariant EKF on Lie groups, an invariant UKF on Lie groups (UKF-LG) has also been developed in [10]. One of the challenges associated with the UKF (whether classical or UKF-LG) lies in the Unscented Transform (UT),
which aims to compute the mean and covariance of the estimate. This issue has been addressed in the literature, with research focusing, for example, on tuning the UT scaling parameters (e.g. [11], [12]). An alternative solution is to use a probabilistic tool known as optimal quantization [13]. In a previous work [15] based on [14], we combined an optimal quantization algorithm with a classical UKF to enhance the filter's performance. In the continuity of these works, we propose in the present paper to augment the UKF on the Lie group SE(2) with an optimal quantization algorithm to achieve similar improvements and apply this contribution to the dual problem of estimation/control using the ILQG of [8]. We argue that the proposed method will enable handling complex trajectory tracking that requires high angular velocities.

The paper is organized as follows. Section II reviews basic definitions about Lie groups. Section III presents the considered model and the linearized matrices that will be used in the invariant LQ regulator and the IEKF prediction step, while also pointing out the issues associated with these linearized matrices. In section IV, we propose a solution that involves replacing the IEKF with an UKF-LG, enhanced by an optimal quantization algorithm. Section V presents simulations that demonstrate the performances across different scenarios of increasing complexity.

## II. PRELIMINARIES

### A. Lie groups

In this paragraph, we recall the definitions of matrix Lie groups and their Lie algebra. A matrix Lie group $G \subset \mathbb{R}^{N \times N}$ is a set of square invertible matrices satisfying the group axioms under matrix multiplication.

Locally about the identity $I$, any vector $\xi \in \mathbb{R}^d$ can be identified with $G$ using the matrix exponential map $\exp(\cdot)$ (where $d = \dim(G)$). Indeed, to any $\xi \in \mathbb{R}^d$, one can associate a matrix of the tangent space of $G$ at $I$, called the Lie algebra $g$ using the operator $[\cdot]^\wedge : \mathbb{R}^d \to g$. Inversely, we have the operator $[\cdot]^\vee : g \to \mathbb{R}^d$. We define the exponential map $\exp : g \to G$ as $\exp([\xi]^\vee) = \chi$. It is a local bijection and we can also define the inverse mapping $\log : G \to g$ given by $\log(\chi) = [\xi]^\vee$.

For notational convenience, we now drop the symbols $[\cdot]^\wedge$ and $[\cdot]^\vee$ (and by a slight abuse of terminology) refer to $\xi$ either as an element of the Euclidean space $\mathbb{R}^d$ or the Lie algebra $g$. Matrix Lie groups, such as SE(2) or SE(3), are extensively used in mobile robotics to describe the pose of a rigid-body.

### B. The Special Euclidean group SE(2)

The definitions in paragraph II-A can be specified in the case where we consider a pose $x = [\theta, X]^\top \in \mathbb{R}^3$ where $\theta \in [-\pi, \pi]$ represents an orientation and $X = [x, y]$ is a vector position. In this case, the matrix $\chi$ is given by

$$ \chi = \begin{pmatrix} \mathcal{R}_{\theta} & \mathbf{X} \\ \mathbf{0} & 1 \end{pmatrix} \in \mathrm{SE}(2) \\ \text{where } \mathcal{R}_{\theta} = \begin{pmatrix} \cos(\theta) & -\sin(\theta) \\ \sin(\theta) & \cos(\theta) \end{pmatrix} \in \mathrm{SO}(2) \quad (1) $$

SO(2) is the Lie group of rotation matrices. Given a vector $\xi_x = [\xi_\theta, \xi_x, \xi_y]^\top$, the exponential map $\exp : \mathbb{R}^3 \to SE(2)$ is defined as

$$ \exp(\xi_x) := \begin{pmatrix} \mathcal{R}_{\xi_\theta} & \mathbf{V}_{\xi_\theta} \\ \mathbf{0} & 1 \end{pmatrix} \\ \text{where } \mathbf{V}_{\xi_\theta} = \frac{1}{\xi_\theta} \begin{pmatrix} \sin(\xi_\theta) & -1 + \cos(\xi_\theta) \\ 1 - \cos(\xi_\theta) & \sin(\xi_\theta) \end{pmatrix} \quad (2) $$

The matrix $\mathbf{V}_{(.)}$ comes from the power series expansion of the exponential map in closed form (see [16] sec. 5.2). The logarithm map $\log : SE(2) \to \mathbb{R}^3$ is given by

$$ \log(\chi) = \begin{pmatrix} \xi_\theta = \theta \\ \begin{bmatrix} \xi_x \\ \xi_y \end{bmatrix} = \mathbf{V}_{\theta}^{-1} \mathbf{X} \end{pmatrix} \quad (3) $$

### C. Random variables on Lie groups

When working on an Euclidean space and characterizing a probability density, a classical framework would be to consider the state vector $x$ to be gaussian distributed, i.e. $x \sim N(\hat{x}, P)$ where $\hat{x}$ is the mean of $x$ and $P$ its covariance matrix. However, to define an uncertainty on a Lie group variable, one can resort to the framework of [17], where $\chi$ is defined as a (left)-equivariant gaussian variable in the sense that

$$ \chi = \hat{\chi} \exp(\xi_x), \quad \xi_x \sim N(0, P) \quad (4) $$

Leading to the compact notation $\chi \sim N_L(\hat{\chi}, P)$ where $\hat{\chi}$ is the mean of $\chi$ and $P$ its covariance matrix. Similarly, a (right)-equivariant gaussian variable can be defined as

$$ \chi = \exp(\xi_x)\hat{\chi}, \quad \xi_x \sim N(0, P) \quad (5) $$

so that $\chi \sim N_R(\hat{\chi}, P)$. In this study, we will consider the left-equivariant gaussian but the development could be applied to the right-equivariant version.

### D. Baker-Campbell-Hausdorff formula

One of the main tools used to manipulate uncertainties on Lie groups is the Baker-Campbell-Hausdorff formula. This is given by

$$ \log(\chi_1 \chi_2) = \log(\exp(a_1) \exp(a_2)) \\ = a_1 + a_2 + \text{higher order terms} \quad (6) $$

$\forall a_1, a_2 \in \mathbb{R}^d$ and where $\exp(a_1) = \chi_1$ and $\exp(a_2) = \chi_2$. The BCH formula is used in [17] to provide a solution for computing the mean and covariance of $\chi_3 \in G$ after a compounding pose $\chi_3 = \chi_1 \chi_2$.

## III. THE INVARIANT LINEAR QUADRATIC GAUSSIAN REGULATOR

In this section, we apply the methodology of [8] to obtain an ILQG regulator, addressing the problem of 2D trajectory tracking. Consider a rigid body moving on a horizontal plane with the state vector being $x = [\theta, X]^\top$. The variable $\theta$ is the orientation of the body and $X$ its position. Let $\{\mathbf{x}_t^*\}_{t=0}^{t_{\mathrm{end}}}$ be a reference trajectory and $\{\mathbf{u}_t^*\}_{t=0}^{t_{\mathrm{end}}}$ a set of reference inputs. The objective of the trajectory tracking is to drive the tracking error to zero such that $\mathbf{x}_t - \mathbf{x}_t^* = \mathbf{0}$.
A. Kinematical model

The discrete kinematical model describing the motion
from time $t$ to $t+1$ with a timestep $dt$ is expressed as

$$
\begin{align}
\mathbf{x}_{t+1} &= f(\mathbf{x}_t, \mathbf{u}_t, \mathbf{w}_t) \nonumber \\
&= \begin{bmatrix}
\theta_t + (u_{\theta_t} + w_{\theta_t})dt \\
x_t + (u_{x_t} + w_{x_t})dt \cos(\theta_t) - (u_{y_t} + w_{y_t})dt \sin(\theta_t) \\
y_t + (u_{y_t} + w_{y_t})dt \sin(\theta_t) + (u_{x_t} + w_{x_t})dt \cos(\theta_t)
\end{bmatrix} \tag{7}
\end{align}
$$

Here, $\mathbf{u}_t = [u_{\theta_t}, u_{x_t}, u_{y_t}]^\top$ is the velocity input vector where $u_{\theta_t}$ denotes the angular velocity whereas $u_{x_t}$ and $u_{y_t}$ are (resp.) the longitudinal velocity along the x-axis of the body local frame and the transversal<fnref n="1" /> velocity along the y-axis of the same frame. The vector $\mathbf{w}_t = [w_{\theta_t}, w_{x_t}, w_{y_t}]^\top \sim \mathcal{N}(\mathbf{0}, \mathbf{Q})$ is a white gaussian noise characterizing the uncertainty acting on the velocities.

In this study we also consider the following observation
model

$$
\mathbf{z}_t = h(\mathbf{x}_t, \mathbf{v}_t) = \begin{pmatrix} 0 & 1 & 0 \\ 0 & 0 & 1 \end{pmatrix} \mathbf{x}_t + \mathbf{v}_t \in \mathbb{R}^2 \quad (8)
$$

This equation models GPS measurements affected by a white
gaussian noise $\mathbf{v}_t \sim \mathcal{N}(\mathbf{0}, \mathbf{R})$ and restricted to a horizontal
plane. We suppose that $\mathbf{v}_t$ is isotropic (i.e. $\mathbf{R} = \sigma_v^2 \mathbf{I}_{2 \times 2}$
where $\sigma_v$ is the standard deviation of the observation noise).

Since our model is different (yet similar) to the one
proposed in the original paper [8], we will derive again
the invariant linearized matrices, starting from the prediction
model (7). Besides, we will make the same assumptions as
in [8] to get the linearized matrices.

B. The Invariant LQ regulator

To get the invariant linearized matrices using the method-
ology of [8], we perform a change of coordinates of the
model (7) thanks to a transformation localized at the refer-
ence trajectory **x**\*. Let us write

$$
\mathbf{\Upsilon}_{\phi} = \begin{pmatrix} 1 & 0 \\ 0 & \mathbf{R}_{\phi} \end{pmatrix} \qquad (9)
$$

where $\phi$ is a given angle. We define the trajectory tracking
error $\bar{\mathbf{x}}_t = \mathbf{x}_t - \mathbf{x}_t^*$ and the input error $\bar{\mathbf{u}}_t = \mathbf{u}_t - \mathbf{u}_t^*$.
The local tracking error is defined as $\bar{\mathbf{x}}_t^{\text{loc}} = \mathbf{\Upsilon}_{-\theta_t^*} \bar{\mathbf{x}}_t$ and
the input error is given by $\bar{\mathbf{u}}_t^{\text{loc}} = \mathbf{\Upsilon}_{-\theta_t^*} \bar{\mathbf{u}}_t$. Then, we can
rewrite the local tracking error at $t+1$ as

$$
\bar{\mathbf{x}}_{t+1}^{\text{loc}} = \mathbf{\Upsilon}_{-\theta_t^*} [f(\mathbf{x}_t, \mathbf{u}_t, \mathbf{w}) - f(\mathbf{x}_t^*, \mathbf{u}_t^*, \mathbf{0})] \quad (10)
$$

By linearizing $f(\mathbf{x}_t, \mathbf{u}_t, \mathbf{w}) - f(\mathbf{x}_t^*, \mathbf{u}_t^*, \mathbf{0})$ with
(assumption i) a first-order Taylor series expansion,
we find that the local tracking error writes

$$
\begin{align}
\bar{\mathbf{x}}_{t+1}^{\text{loc}} & \approx \mathbf{\Upsilon}_{-\theta_{t+1}^*} \left( \frac{\partial f}{\partial \mathbf{x}} \middle|_{\mathbf{x}_t^*, \mathbf{u}_t^*, \mathbf{0}} \bar{\mathbf{x}}_t + \frac{\partial f}{\partial \mathbf{u}} \middle|_{\mathbf{x}_t^*, \mathbf{u}_t^*, \mathbf{0}} \bar{\mathbf{u}}_t + \frac{\partial f}{\partial \mathbf{w}} \middle|_{\mathbf{x}_t^*, \mathbf{u}_t^*, \mathbf{0}} \mathbf{w}_t \right) \tag{11} \\
& \phantom{\approx} \quad + \mathbf{\Upsilon}_{-\theta_{t+1}^*} \left( \frac{\partial f}{\partial \mathbf{x}} \middle|_{\mathbf{x}_t^*, \mathbf{u}_t^*, \mathbf{0}} \bar{\mathbf{x}}_t + \frac{\partial f}{\partial \mathbf{u}} \middle|_{\mathbf{x}_t^*, \mathbf{u}_t^*, \mathbf{0}} \bar{\mathbf{u}}_t + \frac{\partial f}{\partial \mathbf{w}} \middle|_{\mathbf{x}_t^*, \mathbf{u}_t^*, \mathbf{0}} \mathbf{w}_t \right) \nonumber
\end{align}
$$

We get the local linearized matrices $\mathbf{A}_t^*$ and $\mathbf{B}_t^*$ by computing $\frac{\partial f}{\partial \mathbf{x}}|_{\mathbf{x}_t^*, \mathbf{u}_t^*, \mathbf{0}}$, $\frac{\partial f}{\partial \mathbf{u}}|_{\mathbf{x}_t^*, \mathbf{u}_t^*, \mathbf{0}}$ and $\frac{\partial f}{\partial \mathbf{w}}|_{\mathbf{x}_t^*, \mathbf{u}_t^*, \mathbf{0}}$. In [8], the authors compute these matrices by making the assumption of a small angular variation $u_{\theta_t}$ (assumption ii), hence $\sin(u_{\theta_t} dt) \approx u_{\theta_t} dt$ and $\cos(u_{\theta_t} dt) \approx 1$, neglecting the higher order terms $O(dt^2)$. Using this assumption for the model (11), we get the following results:

$$
\begin{align}
\mathbf{\Upsilon}_{-\theta_t^{*+1}} \frac{\partial f}{\partial \mathbf{x}} \middle|_{\mathbf{x}_t^*, \mathbf{u}_t^*, \mathbf{0}} &\approx \underbrace{\begin{pmatrix} 1 & 0 & 0 \\ -u_{y_t}^* dt & 1 & u_{\theta_t}^* dt \\ u_{x_t}^* dt & -u_{\theta_t}^* dt & 1 \end{pmatrix}}_{:= \mathbf{A}_t^*} \begin{pmatrix} 1 & 0 & 0 \\ 0 & \cos(\theta_t^*) & \sin(\theta_t^*) \\ 0 & -\sin(\theta_t^*) & \cos(\theta_t^*) \end{pmatrix} \nonumber \\
&:= \mathbf{\Upsilon}_{-\theta_t^*} \tag{12}
\end{align}
$$

$$
\mathbf{\Upsilon}_{-\theta_t^*+1} \frac{\partial f}{\partial \mathbf{u}} \middle|_{\mathbf{x}_t^*, \mathbf{u}_t^*, \mathbf{0}} \approx dt \mathbf{I}_{3 \times 3} := \mathbf{B}_t^* \quad (13)
$$

$$
\mathbf{\Upsilon}_{-\theta_t^*+1} \frac{\partial f}{\partial \mathbf{w}} \middle|_{\mathbf{x}_t^*, \mathbf{u}_t^*, \mathbf{0}} \approx dt \mathbf{I}_{3 \times 3} := \mathbf{B}_t^* \quad (14)
$$

An LQ control policy is then applied to the linearized
model, which minimizes the quadratic cost function $J(\mathbf{x}, \mathbf{u})$:

$$
J(\mathbf{x}, \mathbf{u}) = \mathbb{E} \left( \sum_{t=0}^{t_{\text{end}}} \left( (\bar{\mathbf{x}}_t^{\text{loc}})^{\top} \mathbf{C} \bar{\mathbf{x}}_t^{\text{loc}} + \bar{\mathbf{u}}_t^{\top} \mathbf{D} \bar{\mathbf{u}}_t \right) \right) \quad (15)
$$

where $\mathbf{C}$ and $\mathbf{D}$ are positive-definite matrices that penalize
the deviations in the tracking and in the input. The input is
corrected as

$$
\mathbf{u}_t = \mathbf{u}_t^* + \mathbf{L}_t^{\text{inv}} \mathbf{\Upsilon}_{-\theta_t^*} (\mathbf{x}_t - \mathbf{x}_t^*) \quad (16)
$$

where $\mathbf{L}_t^{\text{inv}}$ is the gain matrix given by the solution of the
backward Riccati equations. However in practice, the true
state $\mathbf{x}_t$ is unknown and we have to replace it by an estimate
$\hat{\mathbf{x}}_t$ obtained from a Kalman Filter.

C. The IEKF

In this part, the correction step of the filter is overlooked
since we have the same observation model $h(\cdot)$ as in [8]. We
define the estimation error $\tilde{\mathbf{x}}_{t|t} = \hat{\mathbf{x}}_{t|t} - \mathbf{x}_t$ where $\hat{\mathbf{x}}_{t|t}$ denotes
the state estimate at time $t$ knowing the measure at $t$. The
local estimation error is then defined as $\tilde{\mathbf{x}}_{t|t}^{\text{loc}} = \mathbf{\Upsilon}_{-\theta_t^*} \tilde{\mathbf{x}}_{t|t}$.
Following the same reasoning as previously, the invariant
linearized matrices $\mathbf{A}_t$ and $\mathbf{B}_t$ of the EKF can be computed.
Hence, we have

$$
\mathbf{A}_t = \begin{pmatrix} 1 & 0 & 0 \\ -u_{y_t} dt & 1 & u_{\theta_t} dt \\ u_{x_t} dt & -u_{\theta_t} dt & 1 \end{pmatrix} \quad \text{and} \quad \mathbf{B}_t = dt \mathbf{I}_{3 \times 3} \tag{17}
$$

We conclude that these matrices are merely approxima-
tions, and the designed IEKF may perform poorly outside
of assumptions (i) and (ii), in particular when computing
the covariance as $\mathbf{P}_{t+1} = \mathbf{A}_t \mathbf{P}_t \mathbf{A}_t^\top + \mathbf{B}_t \mathbf{Q} \mathbf{B}_t^\top$, which
can lead to inaccuracies. In fact, in modern IEKF approach
using Lie groups [7], the covariance is computed as $\mathbf{P}_{t+1} =$
$\mathbf{F}_t \mathbf{P}_t \mathbf{F}_t^\top + \mathbf{Q}$ with $\mathbf{F}_t = \text{Ad}[(\exp(\mathbf{u}_t dt))^{-1}]$ and where
$\text{Ad}[\cdot]$ is the Adjoint operator of the group $\mathbf{SE}(2)$. The only

<footnote n="1">¹It is often common to consider a non holonomic kinematical model to describe the motion of (for example) a car, which is the case in [8]. In this latter, the kinematical model does not have a transversal shift for the motion of *x* and *y*, thus allowing two inputs only: the angular velocity and the longitudinal velocity. The model (7) is more generic and describes the motion of a holonomic robot moving on a plane such as an omni-directional robot.</footnote>
difference between $A_t$ and $F_t$ is that the latter doesn't assume a small angle assumption for $u_{\theta_t}$. However, computing the covariance as $F_t P_t F_t^\top + Q$ remains an approximation, as it omits higher-order terms (albeit in the context of the BCH formula). To avoid the need for these assumptions, we turn to sigma-point methods.

## IV. PROPOSED METHOD

Since it is just a question of accuracy regarding the computation of the mean and covariance during the prediction step, we propose in this section to replace the IEKF by an UKF-LG [10]. Additionally, we also slightly modify the uncertainty propagation step based on previous works [15]. Note that we do not reconsider the regulator's design, i.e. we intend to keep the linearized reference matrices $A_t^*$ and $B_t^*$ as computed earlier and solely focus on the estimator's design. Nonetheless, we argue that modifying the estimation method will also improve the regulator's performances.

### A. Kinematical model on SE(2)

First, instead of using (7), we can use the SE(2) group counterpart given in [10]. The kinematical model describing the motion writes

$$ \chi_{t+1} = \chi_t \exp((\mathbf{u}_t + \mathbf{w}_t)dt), \quad \mathbf{w}_t \sim \mathcal{N}(\mathbf{0}, \mathbf{Q}) \quad (18) $$

and the observation model can be expressed as

$$ \begin{bmatrix} \mathbf{z}_t \\ 1 \end{bmatrix} = h(\chi_t, \mathbf{v}_t) = \chi_t \begin{bmatrix} \mathbf{0} \\ 1 \end{bmatrix} + \begin{bmatrix} \mathbf{v}_t \\ 0 \end{bmatrix}, \quad \mathbf{v}_t \sim \mathcal{N}(\mathbf{0}, \mathbf{R}) \quad (19) $$

### B. Sigma-point method on Lie groups

The use of a sigma-point method [18] to approximate a density probability after propagation through a nonlinear model has been proven to be more efficient and easier to implement in practice than performing an analytical linearization of the model. The Unscented Transform is a sigma-point method based on the Gauss-Hermite quadrature rule [19]. The idea is to generate a set of points $\{\mathbf{x}_t^{(i)}\}$ ($i = \{0, 1, ...\}$) which captures the distribution of $\mathbf{x}_t$ and propagate each one of them into the model, producing a new set of points, which can be used to compute the statistics of the propagated variable $\mathbf{x}_{t+1} \sim \mathcal{N}(\hat{\mathbf{x}}_{t+1}, \mathbf{P}_{t+1})$.

When propagation lies on a Lie group such as the model (18), the UT can be performed as in [17] where the sigma-points are generated in the Lie algebra, then exploiting the uncertainty model (4). These sigma-points are used to capture the distribution $\chi_{t+1} \sim \mathcal{N}_L(\hat{\chi}_{t+1}, \mathbf{P}_{t+1})$ as accurately as possible. For the mean, it was shown in [17], that propagating it as

$$ \hat{\chi}_{t+1} = \hat{\chi}_t \exp(\mathbf{u}_t dt) \quad (20) $$

is correct up to the fifth order in the sense of the BCH formula. The UKF-LG of [10] is built upon the mean propagation (20) and uses the UT (described below) to approximate the covariance.

Let a covariance matrix $\Sigma \in \mathbb{R}^{n \times n}$. The UT first generates $2n+1$ sigma-points and weights as follows:

$$ \begin{aligned} \xi^{(0)} &= \mathbf{0} \\ \xi^{(i)} &= -\sqrt{3}(\sqrt{\Sigma})_i \quad \forall i = 1, \dots, n \\ \xi^{(i+n)} &= \sqrt{3}(\sqrt{\Sigma})_i \quad \forall i = 1, \dots, n \\ W^{(0)} &= \frac{3-n}{3}, \quad W^{(i)} = \frac{1}{6}, \quad \forall i = 1, \dots, 2n \end{aligned} \quad (21) $$

where $(\sqrt{\Sigma})_i$ denotes the $i^{th}$ column of the square-root matrix of $\Sigma$. The sigma-points are then propagated to compute the covariance $\forall i = 0, \dots, 2n$, exploiting (4):

$$ \begin{aligned} \text{Let } \xi^{(i)} &= [\xi_{\mathbf{x}}^{(i)\top}, \mathbf{w}^{(i)\top}]^{\top} \\ \chi_{t+1}^{(i)} &= \hat{\chi}_t \exp(\xi_{\mathbf{x}}^{(i)}) \exp(\mathbf{u}_t + \mathbf{w}^{(i)}) \\ \xi_{\mathbf{x}}^{(i)} &= \log(\hat{\chi}_{t+1}^{-1}\chi_{t+1}^{(i)}) \\ \mathbf{P}_{t+1} &= \sum_{i=0}^{2n} W^{(i)}(\xi_{\mathbf{x}}^{(i)})(\xi_{\mathbf{x}}^{(i)})^{\top} \end{aligned} \quad (22) $$

We would like to improve the accuracy of the UT for the covariance approximation by focusing on the sigma-points placement involved before their propagation. To do that, we propose a solution based on optimal quantization.

### C. Optimal quantization

Let $\mathcal{P}(\xi)$ any probability density for the random vector $\xi \in \mathbb{R}^n$. An $(N+1)$-quantizer is defined as a mapping $\mathcal{Q}: \mathbb{R}^n \rightarrow \{\xi^{(0)}, \dots, \xi^{(N)}\} \subset \mathbb{R}^n$. The $(N+1)$-quantizer $\mathcal{Q}$ is said to be 2-optimal if it minimizes the expected value of the quantization error (also called distortion):

$$ \mathbb{E}(\|\xi - \mathcal{Q}(\xi)\|_2) = \int_{\mathbb{R}^n} \|\xi - \mathcal{Q}(\xi)\|_2 d\mu(\xi) \quad (23) $$

with $d\mu(\xi) = \mathcal{P}(\xi)d\xi$ a probability measure. In the case where $\mathcal{P}(\xi) = \mathcal{N}(\xi, \Sigma)$, the distortion can be expressed as

$$ \mathbb{E}(\|\xi - \mathcal{Q}(\xi)\|_2) = \sum_{i=0}^{N} \int_{\mathbb{R}^n} \|\xi - \xi^{(i)}\|_2 \mathcal{N}(\hat{\xi}, \Sigma) d\xi \quad (24) $$

The objective of the optimal quantization is to discretize the probability density of $\xi$ with $N+1$ supporting points, while satisfying the minimization of (23) (in general) or (24) (in the specific gaussian case) solely to accurately represent the distribution of the random vector.

### D. Sigma-points readjustment by optimal quantization

The sigma-points generation during the UT is already a $(N+1)$-quantizer of the probability density of $\xi$ (with $N=2n$). However, we have shown in [15] that performing a quantization in the sense of the minimization of (24) provides better results. Hence, we propose to use an optimal quantization algorithm that modifies the set of sigma-points already generated. A method, known as Competitive Learning Vector Quantization (see [14] for more details), is presented in Algorithm 1 and allows to recursively readjust a predefined set of points $\{\xi^{(i)}\}_{i=0}^{2n}$ all while minimizing (24) thanks to a stochastic gradient descent.
**Algorithm 1** Competitive Learning Vector Quantization (CLVQ)

Inputs: {$\xi^{(i)}$}$_{i=0}^{2n}$, $\Sigma$, $k_{\max} \in \mathbb{N}$

1: **for** $k=1$ to $k_{\max}$ **do**

2: Draw $\xi_{\text{rand}}$ from probability distribution $\mathcal{N}(0, \Sigma)$

3: Find the winning index $j = \underset{i}{\operatorname{argmin}} (\|\xi_{\text{rand}} - \xi^{(i)}\|_2)$

4: $\xi^{(i)} \leftarrow \xi^{(i)} \quad \forall i \neq j$

5: $\xi^{(j)} \leftarrow \xi^{(j)} - \Gamma_k(\xi^{(j)} - \xi_{\text{rand}})$

6: **end for**

Outputs: {$\xi^{(i)}$}$_{i=0}^{2n}$

At each iteration $k$, we have $\Gamma_k = \text{diag}(\gamma_{1k}, \dots, \gamma_{nk})$ where each $\gamma_{nk}$ is a small step being part of a sequence $\{\gamma_{nk}\}_{k=1}^{k_{\max}}$ satisfying $\gamma_{nk} \in ]0, 1[$ and

$$ \sum_k \gamma_{nk} = +\infty, \quad \sum_k \gamma_{nk}^2 < +\infty \quad (25) $$

This parameter affects the speed of convergence of the CLVQ algorithm. In [14], an expression is provided to obtain it from a given number of samples and a given size $n$. But for simplicity, a reasonable value can be $\gamma_{nk} = \frac{\epsilon\sqrt{\sum_{n=1}^{n} \epsilon}}{k}$ with $\epsilon \in [0.02, 0.1]$. The choice of $\epsilon$ and $k_{\max}$ are user-defined parameters but some trial and error can be necessary to tune $\epsilon$. The parameter $k_{\max}$ must be large enough to ensure the convergence but not too large to avoid increasing the computation time. It is worth noting that the practitioner can selectively adjust a partial state (not the full one), depending on which uncertainties are most critical to address.

The convergence of the CLVQ algorithm is a consequence of the convergence of the stochastic gradient descent and still applies on Lie groups since sigma-points live in an Euclidean space. More generally, stochastic gradient descent methods have been proven to be applicable to Riemannian manifolds and converge under weak assumptions [20].

### E. Left-UKF-LG with optimal quantization

Consider the model consisting of equations (18)-(19) and the uncertainty on $\chi$ defined by (4). To further enhance the accuracy of the covariance computation, we propose to add the CLVQ algorithm into the UKF-LG similarly to a previous work [15]. Although these latter were developed for a standard UKF, our proposition can be applied on a UKF-LG since the UT on the Lie algebra is a simple UT on a Euclidean space. Thus, Algorithm 1 is added to the Left-UKF-LG to produce this new version, which is described in Algorithm 2.

## V. SIMULATIONS

In this section, we compare the performances of three estimators to solve the dual problem control/estimation:

* the IEKF from [8] adapted to the model (7)

* the original Left-UKF-LG from [10]

* the OQ-Left-UKF-LG proposed in Algorithm 2

For all cases, the invariant LQ policy (16) is applied where we set the same matrices $C = I_{3 \times 3}$ and $D = I_{3 \times 3}$. During

**Algorithm 2** The Optimal Quantization-based Left-UKF on SE(2)

Inputs: $\hat{\chi}_{0|0}$, $\mathbf{P}_{0|0}$, $\mathbf{u}_0$, $\mathbf{Q}$, $\mathbf{R}$, $dt$

1: **for** $t=0$ to $t_{\text{end}}-1$ **do**

2: Compute the mean $\hat{\chi}_{t+1|t}$ using (20)

3: Let $\Sigma = \text{diag}(\mathbf{P}_{t|t}, \mathbf{Q})$, $n=6$

4: Generate sigma-points $\xi^{(i)} = [\xi_{\mathbf{x}}^{(i)}]^{\top}, \mathbf{w}^{(i)}]^{\top}$ and weights using (21)

5: Readjust the set $\{\xi_{\mathbf{x}}^{(i)}\}_{i=0}^{2n}$ using Algorithm 1 with input $\Sigma = \mathbf{P}_{t|t}$

6: Propagate sigma-points and compute the covariance $\mathbf{P}_{t+1|t}$ using (22)

7: **if** measurement $\mathbf{z}_{\text{meas}}$ acquired **then**

8: Let $\Sigma = \text{diag}(\mathbf{P}_{t+1|t}, \mathbf{R})$, $n=5$

9: Generate sigma-points $\xi^{(i)} = [\xi_{\mathbf{x}}^{(i)}]^{\top}, \mathbf{v}^{(i)}]^{\top}$ and weights using (21)

10: Readjust the set $\{\xi_{\mathbf{x}}^{(i)}\}_{i=0}^{2n}$ using Algorithm 1 with input $\Sigma = \mathbf{P}_{t+1|t}$

11: Propagate sigma-points $[\xi_{\mathbf{x}}^{(i)}]^{\top}, \mathbf{v}^{(i)}]^{\top}$ through the observation model $\forall i = 0, \dots, 2n: [\mathbf{z}^{(i)}, 1]^{\top} = h(\hat{\chi}_{t+1|t} \exp(\xi_{\mathbf{x}}^{(i)}), \mathbf{v}^{(i)})$

12: $\hat{\mathbf{z}} = \sum_{i=0}^{2n} W^{(i)} \mathbf{z}^{(i)}$ 
$\mathbf{P}_{\mathbf{z}} = \sum_{i=0}^{2n} W^{(i)} (\mathbf{z}^{(i)} - \hat{\mathbf{z}})(\mathbf{z}^{(i)} - \hat{\mathbf{z}})^{\top}$ 
$\mathbf{P}_{\mathbf{x}\mathbf{z}} = \sum_{i=0}^{2n} W^{(i)} (\xi_{\mathbf{x}}^{(i)})(\mathbf{z}^{(i)} - \hat{\mathbf{z}})^{\top}$ 
$\mathbf{K} = \mathbf{P}_{\mathbf{x}\mathbf{z}}\mathbf{P}_{\mathbf{z}}^{-1}$

13: $\hat{\chi}_{t+1|t+1} = \hat{\chi}_{t+1|t} \exp(\mathbf{K}(\hat{\mathbf{z}} - \mathbf{z}_{\text{meas}}))$ 
$\mathbf{P}_{t+1|t+1} = \mathbf{P}_{t+1|t} - \mathbf{K}\mathbf{P}_{\mathbf{x}\mathbf{z}}\mathbf{K}^{-1}$

14: **end if**

15: **end for**

Outputs: $\{\hat{\chi}_{t|t}\}_{t=0}^{t_{\text{end}}}$, $\{\mathbf{P}_{t|t}\}_{t=0}^{t_{\text{end}}}$

a run, the timestep of the simulation is $dt = 0.01$ sec and the measurement updates occur at the frequency of 1 Hz. Since the model $h(\cdot)$ provides observations on the positions $x$ and $y$, it can be sufficient to apply the optimal quantization on the state variable $\theta$ only. The CLVQ parameters are set as $\epsilon = 0.05$ and $k_{\max} = 100$.

### A. Robustness to initial errors and angular variations

To evaluate the global performance of the proposed solution outside the validity domain of assumptions (i) and (ii) (section III-B), we conduct experiments on four different scenarios. Each scenario represents a reference trajectory $\{\mathbf{x}_t^*\}_{t=0}^{t_{\text{end}}}$ where multiple loops occur, ranging from small angle variations (scenario 1) to large angle variations (scenario 4) and are generated by the sequence of reference inputs $\{\mathbf{u}_t^*\}_{t=0}^{t_{\text{end}}}$. The main characteristics of each scenario are given in Table I. We then perform 1000 Monte-Carlo simulations and assess the values of the quadratic cost function (15). For all estimators and simulations, the state is randomly initialized as $\mathbf{x}_0 = \mathbf{x}_0^* + \mathbf{e}$ with an error $\mathbf{e} \sim \mathcal{N}(0, \mathbf{I})$ and the estimate as $\hat{\mathbf{x}}_{0|0} = \mathbf{x}_0$. The initial covariance is set to $\mathbf{P}_{0|0} = \text{diag}((\pi/6)^2, 0.3^2, 0.3^2)$ and the Kalman noise parameters are consistent with the real noise with covariance matrices
<table><caption>TABLE I: Linear and angular mean velocities of the reference inputs $\mathbf{u}^*$ over the whole trajectory for scenarios 1, 2, 3, and 4.</caption><thead><tr><td>Scenario</td><td>Avr. lin. vel. (m/s)</td><td>Avr ang. vel. (rad/s)</td></tr></thead><tbody><tr><td>1</td><td>2.12</td><td>0.05</td></tr><tr><td>2</td><td>2.12</td><td>0.8</td></tr><tr><td>3</td><td>2.12</td><td>2.41</td></tr><tr><td>4</td><td>2.12</td><td>5.04</td></tr></tbody></table>

being $\mathbf{Q} = \text{diag}((\pi/6)^2, 0.3^2, 0.3^2)$ and $\mathbf{R} = 0.5^2 \mathbf{I}_{2 \times 2}$. All methods process the same sequence of pseudo-random variables for the observation noise $\mathbf{w}_t$ and the sensor noise $\mathbf{v}_t$ to ensure a fair comparison.

The results on the cost $J(\mathbf{x}, \mathbf{u})$ are plotted in Fig. 1. As we can see, the cost is (in average) lower when a

<figure><img src="image_1.png" /><figcaption>Fig. 1: Mean of the cost value over 1000 Monte-Carlo trials for all methods. The lower bounds of the error bars are set to a percentile of 2.5 and the upper bounds are set to a percentile of 97.5, meaning that 95% of the trials lie within the error bars.</figcaption></figure>

sigma-point filter is used for scenarios where high angular variations occur (2, 3 and 4). For the first scenario, there is no significant improvement. But the more we meet large $u_{\theta_t}$ (i.e. the more we move away from assumption (ii)), the more the IEKF lose robustness while the sigma-point

approach remains robust. Furthermore, the OQ-Left-UKF-LG seems slightly better than the standard Left-UKF-LG and the difference becomes more noticeable as the trajectory tracking becomes less easy.

## B. Trajectory tracking and estimation errors correlated to the angular variation

We show in detail the performances of the Left-UKF-LG and the OQ-Left-UKF-LG over the IEKF where the trajectory corresponds to large reference inputs $u_{\theta_t}^*$. For this scenario, the possible angular speed range from 0 to 11 rad/s (see top plot of Fig. 2). As previously, we performed 1000

<figure><img src="image_2.png" /><figcaption>Fig. 2: Average Monte-Carlo of the estimation error (bottom) and trajectory tracking error (middle) as a function of time. The solid lines are the mean of the errors and the dashed lines are the corresponding $3\sigma$ standard deviations. The figure at the top shows the angular velocity reference (black) and the system's angular rate of change (other colors) occurring during this scenario.</figcaption></figure>

Monte-Carlo simulations. The initialization of the hyperparameters was identical to the previous subsection. We computed the average Monte-Carlo of the trajectory tracking and estimation errors, then plotted these results as a function of time (middle and bottom plots of Fig. 2). Conclusions similar to the previous subsection can be drawn from these
results but here we highlight the estimation and tracking errors correlated to the angular speed, giving an idea of values $u_{\theta_t}$ where the proposed solution may be relevant. It is shown that for small $u_{\theta_t}$ (from 0 to ~3 sec), the three methods seem to behave identically, with a slight advantage of the OQ-Left-UKF for the estimation between 1 and 2 sec (bottom plot) and for the tracking between 2 and 3 sec (middle plot). Beyond 3 sec of simulation, the angular speed $u_{\theta_t}$ grows and the difference on the tracking becomes more visible.

### C. Computational efficiency

We evaluate the computational effort by performing 1000 simulations and evaluating the time spent by each method for a simulated runtime of 20 seconds. The average computational time is given in Table II together with the difference given in terms of percentages, with the Left-UKF-LG as the reference. The IEKF computational time is shown to be equal to 11% of the time spent by the Left-UKF-LG. The OQ-Left-UKF-LG computational time is 80% higher than that of the Left-UKF-LG. These results were obtained for the parameter $k_{max} = 100$ and applying the CLVQ on one out of three dimensions.

<table><caption>TABLE II: Average computational time over the whole trajectory for each filter.</caption><thead><tr><th>IEKF</th><th>Left-UKF-LG</th><th>OQ-Left-UKF-LG</th></tr></thead><tbody><tr><td>0.0218 sec</td><td>0.1894 sec</td><td>0.3033 sec</td></tr><tr><td>11%</td><td>100%</td><td>180%</td></tr></tbody></table>

## VI. CONCLUSIONS

In this study, we proposed to tackle the problem of 2D trajectory tracking on SE(2) by an Invariant Linear Quadratic Gaussian regulator and using an Unscented Kalman Filter on Lie groups instead of an Invariant Extended Kalman Filter. The UKF-LG has been enhanced with an optimal quantization algorithm to improve its accuracy. The results show an improvement on the estimation error and a better robustness for trajectory tracking scenarios involving high-angle maneuvers with large noises. Future research should focus on the development of the OQ-Left-UKF-LG on SE(3) for 3D state estimation and trajectory tracking. Another path of investigation could be the improvement of the computational efficiency of the CLVQ, in particular the use of a stopping criterion instead of a number of iteration $k_{max}$. Looking for another method to find the winning index could also be interesting since it is the most time consuming task.

## REFERENCES

[1] M. B. Alatise and G. P. Hancke, "A Review on Challenges of Autonomous Mobile Robot and Sensor Fusion Methods," in IEEE Access, vol. 8, pp. 39830-39846, 2020, doi: 10.1109/ACCESS.2020.2975643.

[2] H. Shakhatreh et al., "Unmanned Aerial Vehicles (UAVs): A Survey on Civil Applications and Key Research Challenges," in IEEE Access, vol. 7, pp. 48572-48634, 2019, doi: 10.1109/ACCESS.2019.2909530.

[3] Brockett, R. W. (1973, August). Lie algebras and Lie groups in control theory. In Geometric Methods in System Theory: Proceedings of the NATO Advanced Study Institute held at London, England, August 27-September 7, 1973 (pp. 43-82). Dordrecht: Springer Netherlands.

[4] N. Aghannan and P. Rouchon, "On invariant asymptotic observers," Proceedings of the 41st IEEE Conference on Decision and Control, 2002., Las Vegas, NV, USA, 2002, pp. 1479-1484 vol.2, doi: 10.1109/CDC.2002.1184728.

[5] S. Bonnabel, P. Martin and P. Rouchon, "Non-Linear Symmetry-Preserving Observers on Lie Groups," in IEEE Transactions on Automatic Control, vol. 54, no. 7, pp. 1709-1713, July 2009, doi: 10.1109/TAC.2009.2020646.

[6] R. Mahony, T. Hamel and J.-M. Pflimlin, "Nonlinear Complementary Filters on the Special Orthogonal Group," in IEEE Transactions on Automatic Control, vol. 53, no. 5, pp. 1203-1218, June 2008, doi: 10.1109/TAC.2008.923738.

[7] Barrau, A., Bonnabel, S. (2018). Invariant kalman filtering. Annual Review of Control, Robotics, and Autonomous Systems, 1(1), 237-257.

[8] S. Diemer and S. Bonnabel, "An invariant Linear Quadratic Gaussian controller for a simplified car," 2015 IEEE International Conference on Robotics and Automation (ICRA), Seattle, WA, USA, 2015, pp. 448-453, doi: 10.1109/ICRA.2015.7139037.

[9] M. R. Cohen, K. Abdulrahim and J. R. Forbes, "Finite-Horizon LQR Control of Quadrotors on SE(2)," in IEEE Robotics and Automation Letters, vol. 5, no. 4, pp. 5748-5755, Oct. 2020, doi: 10.1109/LRA.2020.3010214.

[10] M. Brossard, S. Bonnabel and J.-P. Condomines, "Unscented Kalman filtering on Lie groups," 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Vancouver, BC, Canada, 2017, pp. 2485-2491, doi: 10.1109/IROS.2017.8206066.

[11] Scardua, L. A., Da Cruz, J. J. (2017). Complete offline tuning of the unscented Kalman filter. Automatica, 80, 54-61.

[12] Rhudy, M. (2023). Selection of Tuning Parameters of the Unscented Kalman Filter using Analytical Truth Statistics. In AIAA SCITECH 2023 Forum (p. 2702).

[13] Graf, S., Luschgy, H. (2007). Foundations of quantization for probability distributions. Springer.

[14] Pagès, G., Printems, J. (2003). Optimal quadratic quantization for numerics: the Gaussian case. Monte Carlo Methods Appl., 9(2), 135-165.

[15] V. Pravong, J.-P. Condomines, G. Ö. Lundin and S. Puechmorel, "Unscented Kalman Filter using Optimal Quantization," 2024 27th International Conference on Information Fusion (FUSION), Venice, Italy, 2024, pp. 1-8, doi: 10.23919/FUSION59988.2024.10706531.

[16] Eade, E. (2013). Lie groups for 2d and 3d transformations. URL http://ethaneade.com/lie.pdf, revised Dec, 117, 118.

[17] T. D. Barfoot and P. T. Furgale, "Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems," in IEEE Transactions on Robotics, vol. 30, no. 3, pp. 679-693, June 2014, doi: 10.1109/TRO.2014.2298059.

[18] S. Julier, J. Uhlmann and H. F. Durrant-Whyte, "A new method for the nonlinear transformation of means and covariances in filters and estimators," in IEEE Transactions on Automatic Control, vol. 45, no. 3, pp. 477-482, March 2000, doi: 10.1109/9.847726.

[19] J. Stecha and V. Havlena, "Unscented Kalman filter revisited — Hermite-Gauss Quadrature approach," 2012 15th International Conference on Information Fusion, Singapore, 2012, pp. 495-502.

[20] S. Bonnabel, "Stochastic Gradient Descent on Riemannian Manifolds," in IEEE Transactions on Automatic Control, vol. 58, no. 9, pp. 2217-2229, Sept. 2013, doi: 10.1109/TAC.2013.2254619.