#  PEBO-SLAM: Observer Design for Visual Inertial SLAM with Convergence
Guarantees

Bowen Yi  Chi Jin  Lei Wang  Guodong Shi  Viorela Ila  and Ian R. Manchester
This paper was supported by the Australian Research Council (ARC). L. Wang was
supported by the National Natural Science Foundation of China (62203386), and
Zhejiang Provincial Natural Science Foundation of China (LZ23F030008).
(Corresponding author: Lei Wang)B. Yi, G. Shi, V. Ila and I.R. Manchester are
with the Australian Centre for Robotics, School of Aerospace, Mechanical and
Mechatronic Engineering, The University of Sydney, Sydney, NSW 2006,
Australia. (e-mail<EMAIL_ADDRESS>{guodong.shi, viorela.ila,
<EMAIL_ADDRESS>C. Jin is with the TuSimple Inc., Liangmaqiao
Road, Beijing, China. (e-mail<EMAIL_ADDRESS>L. Wang is with State Key
Laboratory of Industrial Control Technology, Institute of Cyber-Systems and
Control, Zhejiang University, Hangzhou 310027, China (e-mail:
<EMAIL_ADDRESS>
###### Abstract

This paper introduces a new linear parameterization to the problem of visual
inertial simultaneous localization and mapping (VI-SLAM)—without any
approximation—for the case only using information from a single monocular
camera and an inertial measurement unit. In this problem set, the system state
evolves on the nonlinear manifold $SE(3)\times\mathbb{R}^{3n}$, on which we
design dynamic extensions carefully to generate invariant foliations, such
that the problem can be reformulated into online _constant parameter_
identification, then interestingly with linear regression models obtained. It
demonstrates that VI-SLAM can be translated into a linear least squares
problem, in the deterministic sense, _globally_ and _exactly_. Based on this
observation, we propose a novel SLAM observer, following the recently
established parameter estimation-based observer (PEBO) methodology. A notable
merit is that the proposed observer enjoys almost global asymptotic stability,
requiring neither persistency of excitation nor uniform complete
observability, which, however, are widely adopted in most existing works with
provable stability but can hardly be assured in many practical scenarios.

Nonlinear observer, parameter estimation-based observer, SLAM, Least squares

##  1 Introduction

###  1.1 Literature review

Simultaneous localization and mapping (SLAM) is a fundamental problem widely
studied in the robotic and automation communities, as well as in the field of
navigation [25, 9, 31, 33, 43], see [7] for a recent review. In SLAM, two main
aims are concerned to be accomplished concurrently—mapping an unknown
environment, and online estimating the pose (i.e. attitude and position) of a
mobile robot. Nowadays, SLAM is becoming an significantly important part for
unmanned systems in the absence of absolute positioning systems.

The algorithmic approaches at the SLAM back end may generally be classified
into two categories, i.e., smoothing and filtering, and the interested reader
may refer to [30] for a comprehensive comparison. The former is commonly
referred as _maximum a posteriori_ (MAP) estimation, or smoothing and mapping
(SaM), which can historically date back to the pioneer work [22], and often be
represented using factor graphs. In smoothing methods, state estimation is
treated as batch optimization. To be precise, with the aim to estimate the
system state ${\bf x}(k)$ at the current moment $k\in\mathbb{N}_{+}$, we
collect all its past trajectories in an extended state $X:=[{\bf
x}(1)^{\top},\ldots,{\bf x}(k)^{\top}]^{\top}$ and formulate SLAM as
maximizing the posterior under some Gaussian assumptions, given a set of
historical record of measurement $\bar{\bf y}:=\\{{\bf y}(j),j=1,\ldots,k\\}$
as well as the kinematic and observation models. Using the Bayes’ theorem and
a uniform distribution assumption, the MAP estimation reduces to maximum
likelihood estimation, or equivalently a nonlinear least squares problem [7,
Sec. 2]. Compared to filtering, this approach usually has a better accuracy in
particular for large-scaled mappings; however, on the other hand, it has a
serious issue of scalability, i.e., heavy memory consumption and growing size
of the factor graph. Such an issue has been partially solved by utilizing
sparsification [15], Parallel (out-of-core) SLAM [6] and incremental smoothing
[17, 14]. In contrast, the filtering approach relies on recursive or
incremental algorithms to get the pose and features estimation asymptotically,
in which only current pose is in need to be estimated, and thus the dimension
of the pose state will not grow boundlessly over time. This school includes
extended Kalman filter (EKF)-SLAM and many modern algorithms, e.g. FastSLAM,
which combines EKF and particle filtering [25]. An essential part of these
algorithms is their convergence and consistency analysis, the success of which
relies on linearization of nonlinear dynamical models [12]. It sometimes
yields satisfactory performance, but may have inconsistency issues when
starting from a bad initial guess, which is caused by small domains of
attraction in terms of approximation—invoking high nonlinearity of the
associated dynamics and output functions.

While new state-of-the-art techniques continue to come up these years, some
long-standing technical and theoretical challenges are still open. As reviewed
above, a key factor stymieing the performance enhancement in practice may
refer to non-convexity in smoothing and locality in filtering; see for example
[34] for an attempt of understanding the nonlinearity structure in one-step
SLAM. With such a consideration, a natural question openly arises in [13]
where it is stated: “How far is SLAM from a linear least squares problem?” The
main purpose of this paper is to give an affirmative answer to the above
question in the context of visual inertial SLAM _without_ involving any
linearization or approximation. Its importance can hardly be overestimated,
since, on one hand, it may help us have a better understanding of nonlinearity
in SLAM, and on the other hand it provides the possibility to enhance
performance in practice. Indeed, many efforts have been made towards this
target. In [42], the authors consider local submap joining via solving linear
least squares problems, which is an approximation to the optimal full
nonlinear least squares SLAM, though non-convex optimization is adopted to
obtain submaps. Meanwhile, some very recent works [21, 32, 10], from the
filtering perspective, shows that _robo-centric_ SLAM problem, i.e.,
estimating landmark coordinates in the body-fixed frame, can be _exactly_
transformed into state observation of linear time-varying (LTV) systems, via
judiciously selecting alternative outputs and systems state. Then, global
asymptotic convergence can be achieved using LTV Kalman-Bucy filters, if the
robot trajectory guarantees uniform complete observability (UCO). Our paper
continues along this line of research as [21, 32], but with the key difference
of focusing on _world-centric_ SLAM.

In recent years, the nonlinear control community shows growing interests for
SLAM, providing alternative solutions by means of nonlinear observer design.
Indeed, the problem may be regarded as state observation of a nonlinear system
living on the manifold $SE(3)\times\mathbb{R}^{3n}$, whose output function
depends on specific sensors. On the other hand, nonlinear observer on
manifolds is a well-established topic, with special emphasis to matrix Lie
groups [16, 20, 23]. In this paper, we are interested in state observer design
for VI-SLAM, a case with only bearing measurement of features and velocities
available. Similar problems were recently studied in [33], in which the
authors introduce a constructive observer design by lifting to a new symmetry
Lie group $\mathbb{VSLAM}(3)$ in order to make the output function
equivariant. Since the model is not strongly differentially observable [4], in
order to be able to achieve asymptotic stability, some persistency of
excitation (PE) conditions are required for the robot trajectory; see for
example [21, 32] imposing the UCO-type assumptions. It is well known that UCO
of LTV systems is equivalent to PE of some intermediate variables [29].
Besides, the PE and UCO conditions are also indispensable in some related
problems, e.g., locolization using range or direction measurements [11],
velocity estimation using normalized measurement [5] and feature depth
observation [8]. Theoretically, the absence of UCO or PE is yet another source
of inaccuracy and inconsistency—apart from nonlinearity and non-convexity as
illustrated above—in filtering approaches. Intuitively, these excitation
conditions impose _relative motion_ between the mobile robot and features, the
_uniformity_ of which should hold with respect to time. However, such
assumptions may not be satisfied in many practical scenarios, such as, robots
stopping in specific tasks, and features appearing in camera only during a
short interval. Under these circumstances, estimates from the existing SLAM
observers fail to converge to their true values; and observer states may even
diverge in the presence of measurement noise. In contrast, smoothing
approaches only require some “informative” assumptions without the need of
uniformity of time, since it solves optimization in each step directly rather
than in a recursive manner. Overcoming the inconsistency from the lack of
sufficient excitation for the filtering approach is another motivation of the
paper.

###  1.2 Overview and Paper Organization

There are many different ways to formulate SLAM problems, among which
smoothing and filtering/observer are two de facto standard frameworks. In the
former, it is formulated as a large batch stochastic optimization problem; and
in filtering the problem is addressed as information fusion. In this work, we
aim to provide a filtering solution to estimate the entire trajectory (pose)
and the map.

The proposed PEBO-SLAM framework can be distinguished from conventional
smoothing approaches (based on nonlinear optimization) and
incremental/recursive SLAM filters (e.g. nonlinear observers and several
variants of EKFs), i.e., the proposed approach relies on using dynamic
extensions and then treating visual inertial SLAM as a linear squares problem
without any approximation. After obtaining linear regressor equations (LREs),
instead of solving off-line optimization directly we provide a consistent on-
line estimator. The PEBO-SLAM framework enjoys three key merits, i.e., low
computational burden, less data memory, and (almost) global stability
guarantees. Although the algorithm in the paper are tailored for a specific
SLAM problem, i.e., three-dimensional visual inertial SLAM, the PEBO-SLAM
framework is applicable to many other SLAM problems.

The paper is organized as follows. In the remainder of this section, we will
summarize the main contributions, and introduce mathematical notations. In
Sections 2 and 3 we present some preliminaries of PEBO and the problem
formulation studied in the paper, respectively. In Section 4, a novel visual
inertial SLAM observer framework will be designed, and then we propose a
feasible solution in Section 5. It will be followed by some simulation results
in Section 6. Finally, the paper is wrapped up by a brief concluding remark.

###  1.3 Contributions

We consider the mobile robot moving in three-dimensional space, and our task
is to asymptotically estimate the robot pose and feature positions, only using
the information of landmark bearings observed from a single monocular camera,
and the odometry from an inertial measurement unit (IMU). The constructive
nonlinear observer tool which we adopt is the recently proposed _parameter
estimation-based observer_ (PEBO) [28]; see [27] for a generalized version,
and [39] for its geometric interpretation. In the paper, we show that PEBO can
be extended from Euclidean space to Lie matrix groups. We call the proposed
design framework as PEBO-SLAM.

The basic idea in PEBO-SLAM is to translate state estimation into online
_constant_ parameter identification. Though smoothing approaches rely on
parameter estimation, we underline the radical difference between smoothing
and the proposed method. The former relies on batch optimization, in which all
historical poses are estimated in each step, known as full-information
estimation; however, in the proposed method we design a dynamic extension to
generate invariant foliations, and then obtain some “invariance”, with only
the current pose to be estimated on-line. The fact makes the proposed approach
belong to the filtering category.

The main contributions of the paper are threefold.

  * C1

Showing that the VI-SLAM problem can be formulated as online parameter
estimation via a PEBO design on nonlinear manifolds, then obtaining a linear
least squares problem without any approximation.

  * C2

Providing a simple VI-SLAM observer design, which enjoys almost global
asymptotic stability, in contrast to locality in EKF-SLAM. Hence, it overcomes
the issue of inconsistency and inaccuracy from bad initial guess, and enjoys
low computational complexity with linear growth with respect to the number of
features.

  * C3

Relaxing significantly the PE or UCO condition required in some recent
results, e.g., [5, 21, 32], and only requiring an extremely weak assumption,
i.e. interval excitation (IE). It means that PEBO-SLAM can still have
satisfactory accuracy and consistency when the mobile robot moves along some
_non-uniformly_ observable trajectories.

An abridged version of was presented in the 60th IEEE Conference on Decision
and Control [38]. While the conference version primarily focused on the core
idea, this extended version includes several additional contributions,
including the proof of Lemma 1, the convergence rate estimate in Theorem 5.7,
the proof of Theorem 5.11 and more simulation results.

###  1.4 Notations

Throughout the paper, the arguments and subscripts are omitted when clear from
context. $\epsilon_{t}$ represents exponentially decaying terms with proper
dimensions. Given $A\in\mathbb{R}^{n\times n}$ and $S\in\mathbb{R}^{n\times
n}_{\succeq 0}$, the Frobenius norm is defined as
$\|A\|=\sqrt{\mbox{tr}(A^{\top}A)}$, and $S^{1\over 2}$ and
$\mbox{adj}\\{A\\}$ represent the matrix square root and the adjugate matrix,
respectively. The skew-symmetric projector $\mbox{skew}(\cdot)$ is defined as
$\mbox{skew}(A)={1\over 2}(A-A^{\top})$. We use $|\cdot|$ to denote Euclidean
norms of vectors or its induced matrix norm. Given two symmetric matrices $A$
and $B$ in $\mathbb{R}^{n\times n}$, we write $A\preceq B$, if for all ${\bf
x}\in\mathbb{R}^{n}$, ${\bf x}^{\top}A{\bf x}\leq{\bf x}^{\top}B{\bf x}$. We
use $SO(3)$ to represent the special orthogonal group, and
${\mathfrak{so}}(3)$ is the associated Lie algebra as the set of skew-
symmetric matrices satisfying $SO(3)=\\{R\in\mathbb{R}^{3\times
3}|R^{\top}R=I_{3},~{}\det(R)=1\\}$. Given $\mathbf{a}\in\mathbb{R}^{3}$, we
define the operator $(\cdot)_{\times}$ as

$\mathbf{a}_{\times}:=\begin{bmatrix}0&-a_{3}&a_{2}\\\ a_{3}&0&-a_{1}\\\
-a_{2}&a_{1}&0\end{bmatrix}\in{\mathfrak{so}}(3).$

We also consider the special Euclidean group denoted as $SE(3)=\\{{\cal
T}(R,{\bf x})\in\mathbb{R}^{4\times 4}|R\in SO(3),~{}{\bf
x}\in\mathbb{R}^{3}\\}$ with

${\cal T}(R,{\bf x})=\begin{bmatrix}R&{\bf x}\\\ 0&1\end{bmatrix}.$ (1)

The Lie algebra of $SE(3)$ is defined as

${\mathfrak{se}}(3):=\left\\{A\in\mathbb{R}^{4\times
4}\Bigg{|}A=\begin{bmatrix}\bm{\Omega}_{\times}&\mathbf{v}\\\
0&0\end{bmatrix},\bm{\Omega}_{\times}\in\mathfrak{so}(3),{\bf
v}\in\mathbb{R}^{3}\right\\}.$

For any ${\bf x}\in\mathbb{R}^{3}$, ${\bf a}\times{\bf x}$ is the vector cross
product, satisfying ${\bf a}_{\times}{\bf x}={\bf a}\times{\bf x}$. For any
${\bf x}\in\mathbb{R}^{3}/\\{0\\}$, its projector is defined as $\Pi_{{\bf
x}}:=I_{3}-{1\over|{\bf x}|^{2}}{\bf x}{\bf x}^{\top},$ which projects a given
vector onto the subspace orthogonal of ${\bf x}$, since $\Pi_{{\bf x}}{\bf
x}=0$. We define a wedged mapping

${\bf u}^{\wedge}=\begin{bmatrix}\bm{\Omega}_{\times}&{\bf v}\\\
0&0\end{bmatrix}$ (2)

for a vector ${\bf u}:=\mbox{col}(\bm{\Omega},{\bf v})\in\mathbb{R}^{6}$.

##  2 Preliminaries

To streamline the presentation, let us recall some preliminaries about the
PEBO methodology in Euclidean space [28]. We are particularly interested in
estimation of unknown states via recursive algorithms. Consider the nonlinear
model

$\dot{\bf x}=f({\bf x},{\bf u}),\quad{\bf y}=h({\bf x})$ (3)

with the systems state ${\bf x}\in\mathbb{R}^{n_{x}}$, ${\bf
u}\in\mathbb{R}^{n_{u}}$ the input and ${\bf y}\in\mathbb{R}^{n_{y}}$ the
output. The target is to asymptotically estimate unknown ${\bf x}$, assuming
that both ${\bf y}$ and ${\bf u}$ are available. In the context of observer,
the estimate $\hat{\bf x}\in\mathbb{R}^{n_{x}}$ is generated from

$\displaystyle\dot{\eta}$ $\displaystyle~{}=~{}{F}(\eta,{\bf y},{\bf u})$ (4)
$\displaystyle\hat{\bf x}$ $\displaystyle~{}=~{}{N}(\eta,{\bf y},{\bf u})$

with two functions $F$ and $N$ to be designed, such that

$\lim_{t\to\infty}|\hat{\bf x}(t)-{\bf x}(t)|=0.$ (5)

It can be designed in continuous time and then implemented via discretization,
or be designed as $\eta_{k+1}={F}(\eta_{k},{\bf y}_{k},{\bf u}_{k})$ in
discrete time directly.

The most popular estimators in the robotics community may refer to extended
Kalman filter (EKF) and Luenberger-like observers [4]; see [3] for a recent
review of nonlinear observers. In EKFs, the observer state $\eta$ consists of
the estimate $\hat{\bf x}$ of unknown variables and the predicted covariance
estimate $P$, and $F$ contains the prediction and update functions. In
Luenberger-like observers, the state $\eta$ is usually selected as the
estimate $\hat{\bf x}$ directly. If the system satisfies uniform complete
observability along given robot trajectories, we may guarantee the convergence
$|\hat{\bf x}-{\bf x}|\to 0$ locally with asymptotic stability. Note that in
many cases the change of coordinates plays a very important roles in observer
design [18, 4, 41].

In contrast to the design of an asymptotically stable error dynamics in EKF or
Luenberger-like observers, a radically new method called PEBO was proposed in
[28], which relies on generation of invariant foliations to translate the
state estimation problem into one of estimation of constant, unknown
parameters. Its first step is to find a smooth change of coordinate ${\bf
x}\mapsto{\bf z}=\phi({\bf x})\in\mathbb{R}^{n_{z}}$ ($n_{z}\geq n_{x}$), in
which the time derivative is _measurable_ , i.e.

$\dot{\bf z}=H({\bf y},{\bf u})$ (6)

for some function $H$ with proper dimensions. It is equivalent to solve the
partial differential equation (PDE)

${\partial{\phi}\over\partial{\bf x}}({\bf x},{\bf u}){f}({\bf x},{\bf
u})=H({h}({\bf x}),{\bf u}).$ (7)

Then, we establish, via introducing a pure integral action

$\dot{\bm{\xi}}=H({\bf y},{\bf u}),$ (8)

the relation

$\phi({\bf x}(t))=\bm{\xi}(t)+{\bm{\theta}},\quad\forall t\geq 0$ (9)

with a _constant_ vector $\bm{\theta}:=\phi({\bf x}(0))-\bm{\xi}(0).$ If
$\bm{\theta}$ is known and $\phi$ is injective, we would get the true state as
${\bf x}={\phi}^{\mathtt{L}}(\bm{\xi}+\bm{\theta},{\bf y})$ with
${\phi}^{\mathtt{L}}$ the left inverse of $\phi$. Hence, the remaining task is
to identify the constant vector $\bm{\theta}$ online, denoted as
$\hat{\bm{\theta}}$, then obtaining the estimate $\hat{\bf
x}={\phi}^{\mathtt{L}}(\bm{\xi}+\hat{\bm{\theta}},{\bf y}).$ To this end, we
rely on the existence of the regression model111High-order derivatives of the
output $y$ may also be involved to yield a regression model. For such a case,
some linear time-invariant filter can be used to remove the difficulty of
calculate dirty derivatives [29].

${\bf y}={h}\big{(}{\phi}^{\mathtt{L}}(\bm{\xi}+\bm{\theta},{\bf y})\big{)}$
(10)

with the signals $\bm{\xi}$ and ${\bf y}$ available. An online parameter
estimator for the regression model (10) and the dynamic extension (8), with
the observer output $\hat{\bf
x}={\phi}^{\mathtt{L}}(\bm{\xi}+\hat{\bm{\theta}},{\bf y})$, is called a PEBO
for the system (3). In Section 4, we extend this idea from Euclidean space to
the special Euclidean group, and then apply to the VI-SLAM problem.

###### Remark 1

The key idea in PEBO is to use the pure integral action (8) to generate an
invariant foliation $\\{({\bf x},\bm{\xi})\in\mathbb{R}^{2n_{x}}|\phi({\bf
x})=\bm{\xi}+\bm{\theta},\;\bm{\theta}\in\mathbb{R}^{n_{x}}\\}$ [39], and then
an online estimator is needed to identify the leaf of the foliation in which
the systems state lives. Fig. 1 presents an intuitive illustration.

Figure 1: A geometric illustration of PEBOs

###### Remark 2

The PDE (7) in PEBO is similar to the one in nonlinear contracting observers
[24, 41], i.e., ${\partial\phi\over\partial{\bf x}}({\bf x},{\bf u})=H({\bf
x},h({\bf x}),{\bf u})$. The difference relies on that in the latter
$H(\cdot)$ is also a function of the transformation $\phi({\bf x})$, and we
impose a contraction (a.k.a. incremental exponential stability) constraint in
the new coordinate. This fact provides a possible way to extend the nonlinear
contracting observer framework in [41] only requiring incremental
stability—but not asymptotic—in the transformed coordinate.

##  3 Problem Statement

###  3.1 Kinematic Model

The kinematics of a rigid body robot with is given by

$\dot{\bf x}=R{\bf v},\quad\dot{R}=R\bm{\Omega}_{\times}.$ (11)

Nomenclature $\hat{(\cdot)}$ Estimate of a variable or parameter
$\tilde{(\cdot)}$ Estimation error $I_{n}$ ${n\times n}$ identity matrix
$\small\\{{\cal I},{\cal B},{\cal V}\\}$ Frames of inertial, body and dynamic
extension ${\bf x}(t)\in\mathbb{R}^{3}$ Robot position at $t$ ${\bf
v}(t)\in\mathbb{R}^{3}$ Linear velocity in $\\{{\cal B}\\}$
$\bm{\Omega}(t)\in\mathbb{R}^{3}$ Rotational velocity in $\\{{\cal B}\\}$
$R(t)\in SO(3)$ Robot attitude matrix $\tensor[^{I}]{R}{{}_{B}}$ $X(t)\in
SE(3)$ Rigid-body pose $X:={\cal T}(R,{\bf x})$ ${}^{v}X(t)\in SE(3)$ Dynamic
extended state ${}^{v}X:={\cal T}(Q,\bm{\xi})$ ${}^{c}X\in SE(3)$ Constant
rigid transformation ${}^{v}\mathbf{T}_{I}$ with the decomposition
${}^{c}X:={\cal T}({}^{c}Q,{}^{c}\bm{\xi})$ $Q(t)\in SO(3)$ Attitude matrix in
$\\{{\cal V}\\}$ $\bm{\xi}(t)\in\mathbb{R}^{3}$ Robot position in $\\{{\cal
V}\\}$ ${\bf u}(t)\in\mathbb{R}^{6}$ Velocity ${\bf
u}=\mbox{col}(\bm{\Omega},{\bf v})$ in $\\{{\cal B}\\}$
${\bm{\ell}}_{i},{}^{v}\bm{\ell}_{i}\in\mathbb{R}^{3}$ Position of the $i$-th
landmark in $\\{{\cal I}\\}$, $\\{{\cal V}\\}$ ${\bf y}_{i},{}^{v}{\bf
y}_{i}\in\mathbb{R}^{3}$ Bearing of the $i$-th landmark in $\\{{\cal B}\\}$,
$\\{{\cal V}\\}$ ${\cal N}\subset\mathbb{N}$ Index set of landmarks
$\\{1,\ldots,n\\}$

All the definitions of symbols and the spaces where they live in can be found
in the table of Nomenclature. We assume that there are $n$ feature points
appearing in the field view of camera, the coordinates $\bm{\ell}_{i}$ of
which in the inertial frame $\\{{\cal I}\\}$ are constant, thus satisfying

$\dot{\bm{\ell}}_{i}=0,\quad i\in{\cal N}:=\\{1,\ldots,n\\}\subset\mathbb{N}.$
(12)

The robot position ${\bf x}$ is the coordinate of the origin of the body-fixed
frame $\\{{\cal B}\\}$ relative to the origin of the inertial frame $\\{{\cal
I}\\}$. Here, we assume that the velocities ${\bf v}$ and $\Omega$ are bounded
for $t\geq 0$, and guarantee the dynamics (11) forward complete. The rigid
body pose contains the attitude $R\in SO(3)$ and the position ${\bf
x}\in\mathbb{R}^{3}$, and with a slight abuse of notation we use $X\in SE(3)$
to represent the pose, which is given by $X:={\cal T}(R,{\bf x})$ with the
function ${\cal T}$ defined in (1). Hence, we have its dynamics

$\dot{X}=X{\bf u}^{\wedge}$ (13)

with ${\bf u}=\mbox{col}(\bm{\Omega},{\bf v})$ containing rotational and
linear velocities. In the visual inertial SLAM problem only monocular cameras
and IMUs are equipped on robotics, only providing the information of landmark
bearings and the velocity ${\bf u}$. Then, the system output is given by

${\bf y}_{i}=R^{\top}{\bm{\ell}_{i}-{\bf x}\over|\bm{\ell}_{i}-{\bf x}|}.$
(14)

For convenience, we write all bearing measurements and landmark coordinates in
vectors

$\displaystyle\bm{Y}:=\begin{bmatrix}{\bf y}_{1}\\\ \vdots\\\ {\bf
y}_{n}\end{bmatrix},\quad\bm{L}:=\begin{bmatrix}\bm{\ell}_{1}\\\ \vdots\\\
\bm{\ell}_{n}\end{bmatrix}.$

Though the dynamics (13) is linear, the output functions ${h}_{i}$ are highly
nonlinear and the state space is a nonlinear manifold, thus making (13)-(14) a
nonlinear system.

###  3.2 Problem Statement

In this paper, we are concerned with the VI-SLAM problem, i.e., estimating the
pose $X(t)\in SE(3)$ and the landmark coordinates
$\bm{\ell}_{i}\in\mathbb{R}^{3}$ ($i\in{\cal N}$) from the measurement (14) in
real time, under the kinematic constraint (11), and the criterion is to make
their estimates as close to the true values as possible.

As mentioned in Introduction, there are many merits to solve the above problem
by employing the filter/observer approach, e.g., high computational efficiency
and low data memory usage. Along this line, our task becomes the following
deterministic observer design for VI-SLAM.

Problem 1. (Visual inertial SLAM observer) Consider the dynamical model (11)
with the output $\bm{Y}$ and the input ${\bf u}$. Design a continuous-time
observer in the form

$\displaystyle\dot{\eta}$ $\displaystyle~{}=~{}F(\eta,\bm{Y},{\bf u})$ (15)
$\displaystyle(\hat{X},\hat{\bm{L}})$ $\displaystyle~{}=~{}N(\eta,\bm{Y},{\bf
u})$

with the variables $\hat{X}\in SE(3)$ and $\hat{\bm{L}}\in\mathbb{R}^{3n}$,
and the functions $F$ and $N$ to be designed, guaranteeing

$\lim_{t\to\infty}\Big{[}\big{\|}\hat{X}(t)-X(t)\big{\|}+\big{|}\hat{\bm{L}}(t)-{\bm{L}}\big{|}\Big{]}=0,$
(16)

under some “informative trajectories” of the robot, and identify the
convergence condition of the proposed observer. $\triangleleft$

For on-line estimation problems, it is widely recognized that excitation
conditions can be used to characterize “informative trajectories”. Here, let
us recall some definitions as follows, including persistency of excitation
(PE) and interval excitation (IE). Clearly, IE is strictly weaker than PE,
since the former does not require uniformity in time.

###### Definition 1

(PE, IE) A bounded signal $\phi:\mathbb{R}_{+}\to\mathbb{R}^{n}$ is

  * -

$(T,\delta)$-PE, if

$\int_{t}^{t+T}\phi(s)\phi^{\top}(s)ds\succeq\delta I_{n},\quad\forall t\geq
0$ (17)

for some $T>0,\delta>0$.

  * -

$(t_{0},t_{c},\delta)$-IE if there exist $t_{0}\geq 0$ and $t_{c}\geq 0$ such
that

$\int_{t_{0}}^{t_{0}+t_{c}}\phi(s)\phi^{\top}(s)ds\succeq\delta I_{n}$ (18)

for some $\delta>0$. $\triangleleft$

An LTV system $\dot{\bf x}=A(t){\bf x}$, ${\bf y}=C(t){\bf x}$ is uniformly
completely observable (UCO), if the matrix
$[C(\cdot)\Phi_{A}(\cdot,t_{0})]^{\top}$ is PE with $\Phi_{A}(\cdot,\cdot)$
the state transition matrix [29].

##  4 PEBO-SLAM Framework

###  4.1 A Parameter Estimation Approach to VI-SLAM

In this section, we propose the PEBO-SLAM framework to achieve (16).

As shown above, one of the key steps in PEBO is to generate invariant
foliations among the system states and dynamic extensions. In our case, the
systems state evolves on the nonlinear manifold $SE(3)\times\mathbb{R}^{3n}$.
In the SLAM context, we construct a dynamic extension

${}^{v}\dot{X}={}^{v}X{\bf u}^{\wedge}$ (19)

with the decomposition

${}^{v}X:={\cal T}(Q,\bm{\xi})\in SE(3),$ (20)

which resembles the pure integral action (8) in Euclidean space. We refer the
pose ${}^{v}X$ living in the dynamic extension frame $\\{{\cal V}\\}$.

We have the algebraic relation in Lemma 1 below, which may be viewed as an
extension of the key identity (9) in PEBO from Euclidean space to matrix Lie
groups. A similar idea was employed in our previous work [40].

###### Lemma 1

Consider the dynamics (11). The dynamic extension (19) is forward complete,
and there exists a constant matrix ${}^{c}X={\cal
T}({}^{c}Q,{}^{c}\bm{\xi})\in SE(3)$ satisfying

${}^{v}X(t)={}^{c}XX(t),\quad\forall t\geq 0,$ (21)

with ${}^{c}Q=Q(0)R(0)^{-1}$ and ${}^{c}\bm{\xi}=\bm{\xi}(0)-{}^{c}Q{\bf
x}(0)$.

######  Proof 4.1.

Define an error variable $E(X,{}^{v}X):={}^{v}XX^{\top},$ which lives on
$SE(3)$. Its time derivative is given by

$\dot{E}={}^{v}X{\bf u}^{\wedge}X^{-1}-{}^{v}XX^{-1}\dot{X}X^{-1}=0.$

Thus, there exists a constant matrix ${}^{c}X\in SE(3)$ satisfying

$E(X(t),{}^{v}X(t))={}^{c}X,\quad\forall t\geq 0,$

which may be written equivalently as (21).222This relationship is called in
[20] that the systems (13) and (19) are $E$-synchronous. By re-arranging terms
in ${}^{v}X(0)={}^{c}XX(0)$, it is straightforward to get

$Q_{c}:=Q(0)R(0)^{-1},\quad{}^{c}\bm{\xi}:=\bm{\xi}(0)-{}^{c}Qx(0).$

From the forward completeness assumption of the dynamics (11), as well as
invoking the identity (21), we conclude that the dynamic extension (19) is
also forward complete.

The above lemma shows that the open-loop dynamic extension (19) and the
kinematics (11) admit an affine relationship (21)—more precisely, there is a
constant rigid transformation ${}^{v}\mathbf{T}_{I}$ between the frames
$\\{{\cal I}\\}$ and $\\{{\cal V}\\}$—by means of which we reformulate the
state estimation of $X(t)$ into the problem of online _constant_ parameter
identification of ${}^{c}X\in SE(3)$.

In the following, we show the unknown constant matrix ${}^{c}X={\cal
T}({}^{c}Q,{}^{c}\bm{\xi})$ verifies a nonlinear regression model.

######  Lemma 4.2.

The functions of unknown variables $({}^{c}X,\bm{L})$, defined by

${}^{v}\bm{\ell}_{i}:={}^{c}\bm{\xi}+{}^{c}Q\bm{\ell}_{i},$ (22)

are _constant_ , and verify the algebraic equation

${\bf
y}_{i}=Q^{\top}{{}^{v}\bm{\ell}_{i}-\bm{\xi}\over|{}^{v}\bm{\ell}_{i}-\bm{\xi}|},$
(23)

with $Q,\bm{\xi}$ given in (20).

######  Proof 4.3.

According to (21) and (22), we have

$\displaystyle{}^{v}\bm{\ell}_{i}$
$\displaystyle~{}=~{}{}^{c}\bm{\xi}_{c}+{}^{c}Q\bm{\ell}_{i}$
$\displaystyle~{}=~{}[{}^{c}\bm{\xi}+{}^{c}Q{\bf
x}]+[{{}^{c}}QR]R^{\top}[\bm{\ell}_{i}-{\bf x}]$
$\displaystyle~{}=~{}\bm{\xi}+QR^{\top}(\bm{\ell}_{i}-{\bf x}),$

and thus, from some basic geometric relation, ${}^{v}\bm{\ell}_{i}$ can be
viewed as the feature point coordinates in the dynamic extension frame
$\\{{\cal V}\\}$. Correspondingly, the bearings in $\\{{\cal V}\\}$ can be
defined as

${}^{v}{\bf
y}_{i}:=Q^{\top}{{}^{v}\bm{\ell}_{i}-\bm{\xi}\over|{}^{v}\bm{\ell}_{i}-\bm{\xi}|}.$

Some simple calculations show that

$\displaystyle{}^{v}{\bf y}_{i}$
$\displaystyle~{}=~{}Q^{\top}{QR^{\top}(\bm{\ell}_{i}-{\bf
x})\over|QR^{\top}(\bm{\ell}_{i}-{\bf x})|}$
$\displaystyle~{}=~{}R^{\top}{\bm{\ell}_{i}-{\bf x}\over|\bm{\ell}_{i}-{\bf
x}|}$ $\displaystyle~{}=~{}{\bf y}_{i},$

thus yielding ${}^{v}{\bf y}_{i}\equiv{\bf y}_{i}$.333An intuitive
interpretation to ${}^{v}{\bf y}_{i}\equiv{\bf y}_{i}$ is that the
transformation (21) of the ambient from $\\{{\cal I}\\}$ to $\\{{\cal V}\\}$
does not change the _relative_ transformation from a robot to a landmark.
Hence, we have verified (23).

######  Remark 4.4.

Eqs. (23)-(22) provides a nonlinear regressor w.r.t. the unknown constant
matrix ${}^{c}X$, with ${\bf y}_{i}$, $Q$ and $\bm{\xi}$ _available_. Despite
being quite simple, the above lemmata show estimation of time-varying states
may be translated into online parameter estimation of ${}^{c}X$ and
${}^{v}\bm{\ell}_{i}$ successively. To the best of the authors’ knowledge,
such a fact has not been explored in SLAM filters design, which, however,
provides possibility to relax significantly the sufficient excitation
conditions in existing approaches, and to get global convergence to overcome
inconsistency and inaccuracy from bad initial guesses.

In terms of the above lemmata, we reformulate SLAM into the following
consistent online estimation problem.

Problem 2 (PEBO-SLAM) Consider the dynamic extension (19), with available
signals ${}^{v}X,{\bf u}$ and $\bf Y$. Design a globally convergent online
identifier for ${}^{v}\bm{\ell}_{i}$ and ${}^{c}X$ recursively. ∎

If the above problem is solvable with the estimates
${}^{v}\hat{\bm{\ell}}_{i}$ and ${}^{c}\hat{X}$, then

$\hat{X}={}^{c}\hat{X}^{\top}{}^{v}X,\qquad\begin{bmatrix}\hat{\bm{\ell}}_{i}\\\
1\end{bmatrix}={}^{c}\hat{X}^{\top}\begin{bmatrix}{}^{v}\hat{\bm{\ell}}_{i}\\\
1\end{bmatrix}$

provide a feasible solution to Problem 1. It is well known that the online
estimation of constant parameters is much easier than the one of time-varying
states. This is one of the motivations of the paper. We call the parameter
identifier (to be designed) with the dynamic extension (19) as PEBO-SLAM.

Throughout the paper, we need the following assumptions.

###### Assumption 1

The robot trajectory $X$ guarantees that 1) $\Pi_{Q{\bf y}_{i}}$ is IE for all
the landmarks to be mapped; and 2) the origin of $\\{{\cal B}\\}$ never
coincides with any feature points.

Let us define the vectors

${\bm{r}}_{i}:=\bm{\ell}_{i+1}-\bm{\ell}_{i},~{}\forall i\in{\cal
N}\backslash\\{n\\},$

and see Fig. 2 for illustration.

###### Assumption 2

There exists $n_{\ell}\geq 3$ such that ${\bm{r}}_{i}\times{\bm{r}}_{j}\neq
0,~{}\mbox{for~{}}i\neq j,~{}i,j\in\\{1,\ldots,n_{\ell}\\}.$

Figure 2: Illustration of the frames in the proposed design (The dynamic
extension frame is a fixed one with ideal measurements; in the above figure,
it is a slowly moving frame due to the drifts stemmed from odometry errors.)

###  4.2 A Linear Least Squares Solution

In this subsection, we show that the nonlinear regressors (23) can be
equivalently written as linear regressors w.r.t. ${}^{v}\bm{\ell}_{i}$.

When the $i$-th feature point is visible in the camera view, invoking (23)
yields for $\forall t\geq 0$

$Q{\bf
y}_{i}={{}^{v}\bm{\ell}_{i}-\bm{\xi}\over|{}^{v}\bm{\ell}_{i}-\bm{\xi}|},$

and after straightforward calculations we obtain

$Q{\bf y}_{i}[Q{\bf
y}_{i}]^{\top}({}^{v}\bm{\ell}_{i}-\bm{\xi})={}^{v}\bm{\ell}_{i}-\bm{\xi}.$

Noting that ${\bf y}$ is a unit vector (i.e. $|{\bf y}|=1$), we thus obtain

$\Pi_{Q{\bf y}_{i}}\cdot[{}^{v}\bm{\ell}_{i}-\bm{\xi}]=0,$

which can be expressed as the linear regression models

$\bm{q}_{i}(t)=\phi_{i}^{\top}(t)\cdot{}^{v}\bm{\ell}_{i},$ (24)

with the measurable regression matrix $\Pi_{Q(t){\bf y}_{i}(t)}$ and the
regressor outputs $q_{i}$, which are defined as $\phi_{i}:=\Pi_{Q{\bf
y}_{i}}^{\top}$ and $\bm{q}_{i}:=\Pi_{Q{\bf y}_{i}}\cdot\bm{\xi}$,
respectively.

If the feature point is unavailable in the view during a bounded interval
${\cal I}\subset[0,\infty)$, we may define $\phi_{i}:=0_{3\times 3}$ and
$\bm{q}_{i}=0_{3}$. Then, it is easy to verify that the linear regressors (24)
still hold true, though not providing any information.

Hence, we unify all landmarks in the linear regression models (24) with the
signals

$\displaystyle\phi_{i}$ $\displaystyle~{}:=~{}\left\\{\begin{aligned}
&\Pi_{Q(t){\bf y}_{i}(t)}^{\top}&~{}~{}~{}~{}~{}~{}&\mbox{visible}\\\
&0_{3\times 3}&&\mbox{invisible}\end{aligned}\right.$ (25)
$\displaystyle\bm{q}_{i}$ $\displaystyle~{}:=~{}\left\\{\begin{aligned}
&\Pi_{Q(t){\bf y}_{i}(t)}\bm{\xi}(t)&~{}&\mbox{visible}\\\
&0_{3}&&\mbox{invisible}\end{aligned}\right.$

For the $i$-th landmark, the estimation of ${}^{v}\bm{\ell}_{i}$ can be solved
via the linear least squares problem

${}^{v}\hat{\bm{\ell}}_{i}~{}=~{}\underset{{}^{v}\bm{\ell}_{i}\in\mathbb{R}^{3}}{\arg{\mbox{min}}}\sum_{k}\Big{|}\Pi_{Q(k){\bf
y}_{i}(k)}\cdot(\bm{\xi}(k)-{}^{v}\bm{\ell}_{i})\Big{|}^{2}$ (26)

for a few moments in the visible interval $k\in[t_{a},t_{b}]$.

Note the above least squares problem is solvable in at least two steps, since
the non full rankness of $\Pi_{Q{\bf y}_{i}}$ at any single moment. Indeed,
its rank equals to two all the time when being visible. Note that the above
least squares problem is presented in the deterministic sense. When
considering the stochastic noise with Gaussian distribution of the bearing
output (14), the distribution cannot be preserved in the linear regression
model (24) after nonlinear transformation.

######  Remark 4.5.

In the above analysis, we use the projector to obtain the linear regression
models (24) from bearings in the dynamic extension frame. This technical trick
was widely used in the literature, e.g., bearing-only formation, range
estimation [11], and navigation [36]. An alternative to generate a linear
regressor may be found in [21] by augmenting both the position
${}^{v}\bm{\ell}_{i}$ and its range $|{}^{v}\bm{\ell}_{i}|$ in the systems
state. Recently, a new linear parameterization to the feature range in the
body-fixed frame $\\{{\cal B}\\}$ is proposed in [37] using linear stable
filters.

######  Remark 4.6.

A sketch of key steps in the proposed PEBO-SLAM framework is given as follows,
based on which we present a feasible observer solution in the next section.

  * S1

Implement the dynamic extension

${}^{v}\dot{X}={}^{v}X{\bf u}^{\wedge},\quad\forall t\geq 0,$

with ${\bf u}$ defined in (2) the velocities from IMUs, and the pose in the
frame of dynamic extension ${}^{v}X={\cal T}(Q,\bm{\xi})$.

  * S2

For each visible landmark, solve the least square problem in (26) for a few
moments in the visible interval $k\in[t_{a},t_{b}]$.

  * S3

Fuse information of ${}^{v}X$ and ${}^{v}\hat{\bm{\ell}}_{i}$ in order to get
robust estimates $\hat{X}$ and $\hat{\bm{\ell}}_{i}$ in the inertial frame, in
particular for the case with imperfect odometry.

##  5 A Feasible Observer Design

After formulating VI-SLAM as a linear least squares problem, there are many
approaches to solve it. In practice, measurement noise should be taken into
account, and thus the dynamic extension (19) may have an accumulation of
errors, thus making ${}^{c}X$ a _slowly time-varying parameter_ rather than a
real constant. To alleviate such a robustness issue, we focus on designing
consistent on-line observers.

In this section, we propose a feasible observer design to solve Problem 2. The
scheme consists of two cascaded observers, i.e., a mapping observer in
$\\{{\cal V}\\}$ in Theorem 5.7—which estimates the landmarks
${}^{v}\bm{\ell}_{i}$ in the dynamic extension frame—and a localization and
mapping observer, summarized in Theorem 5.11, to express the estimates of pose
and landmarks in the inertial frame $\\{{\cal I}\\}$. See Fig. 3 for the
overall schematic block diagram of the proposed design.

Figure 3: Block diagram of the proposed visual inertial SLAM observer

###  5.1 Mapping Observer in the Dynamic Extension Frame

In this subsection, we consider on-line observer design to estimate
${}^{v}\bm{\ell}_{i}$, which corresponds to the step S2 in Remark 4.6.

It is well known that standard gradient descent or least square algorithms
require some PE conditions to guarantee consistent convergence [29].
Intuitively, it requires that landmarks appear in camera view persistently,
and the robot keeps moving w.r.t. landmarks, which is rarely satisfied in
practice. In the sequel, we show that in terms of the LREs (24) the
restrictive excitation conditions can be relaxed significantly. The main
technical route is using the DREM technique [1, 26] to obtain decoupled
regressors, and implicitly combining with “integral type” historical
information. We have the following.

Mapping observer in the dynamic extension frame:

$\dot{\eta}_{\Sigma}=F_{\Sigma}(\eta_{\Sigma},\bm{q}_{i},\phi_{i}),\quad{}^{v}\hat{\bm{\ell}}_{i}=N_{\Sigma}(\eta_{\Sigma}),$
(27)

in which the exogenous signals $\phi_{i},\bm{q}_{i}$ are introduced in (25)
with the observer internal state
$\eta_{\Sigma}:=({\bm{q}_{i}^{3},\Phi_{i},\bm{\chi}_{i},\omega_{i},{}^{v}\hat{\bm{\ell}}_{i}})$,
and the functions $F_{\Sigma}$ and $N_{\Sigma}$ are given by the state-space
model

$\Sigma:\left\\{~{}\begin{aligned}
{\dot{\bm{q}}}_{i}^{e}&~{}=~{}-\alpha_{i}\bm{q}_{i}^{e}+\alpha_{i}\phi_{i}\bm{q}_{i},~{}\bm{q}_{i}^{e}(0)=0\\\
\dot{\Phi}_{i}&~{}=~{}-\alpha_{i}\Phi_{i}+\alpha_{i}\phi_{i},~{}\Phi_{i}(0)=0\\\
\dot{\bm{\chi}}_{i}&~{}=~{}\Delta_{i}(\bm{Y}_{i}-\Delta_{i}\bm{\chi}_{i})\\\
\dot{\omega}_{i}&~{}=~{}-\Delta_{i}^{2}\omega_{i},~{}\omega_{i}(0)=1\\\
{}^{v}\dot{\hat{\bm{\ell}}}_{i}&~{}=~{}\gamma_{i}\Delta_{i}^{e}(\bm{Y}_{i}^{e}-\Delta_{i}^{e}\cdot{}^{v}\hat{\bm{\ell}}_{i})\end{aligned}\right.$
(28)

with

  * -

the functions:

$\displaystyle\Delta_{i}$ $\displaystyle~{}:=~{}\det\\{\Phi_{i}\\}$ (29)
$\displaystyle\bm{Y}_{i}$
$\displaystyle~{}:=~{}\mbox{adj}\\{\Phi_{i}\\}\bm{q}_{i}^{e}$
$\displaystyle\Delta_{i}^{e}$ $\displaystyle~{}:=~{}\Delta_{i}+k_{\tt
I}^{i}(1-\omega_{i})$ $\displaystyle{\bm{Y}}_{i}^{e}$
$\displaystyle~{}:=~{}\bm{Y}_{i}+k_{\tt
I}^{i}\big{[}\bm{\chi}_{i}-\omega_{i}\bm{\chi}_{i}(0)\big{]}.$

  * -

the gains: $\alpha_{i}>0,~{}\gamma_{i}>0,~{}k_{\tt I}^{i}>0.$

∎

The above observer enjoys the following provable convergence guarantees.

######  Theorem 5.7.

(Mapping in the dynamic extension frame) The mapping observer (28)-(29)
guarantees

  * 1)

(Internal stability) All the internal states are bounded.

  * 2)

(Element-wise monotonicity) For all $t_{a}\geq t_{b}\geq 0$,

$|{}^{v}\hat{\ell}_{i,j}(t_{a})-{}^{v}\ell_{i,j}|\leq|{}^{v}\hat{\ell}_{i,j}(t_{b})-{}^{v}\ell_{i,j}|.$

  * 3)

(GES under IE) Assuming that $\Pi_{Q{\bf y}_{i}}$ is
$(t_{0},t_{c},\delta)$-IE, we have

$\lim_{t\to\infty}|{}^{v}\hat{\bm{\ell}}_{i}(t)-{}^{v}\bm{\ell}_{i}|=0\quad\mbox{(exp.)}$
(30)

globally.

  * 4)

(Convergence rate) Under the above IE condition, the exponential convergence
rate is not less than

$\gamma_{\star}:=\gamma_{i}(k_{\tt
I}^{i})^{2}\left(1-\exp\left(-{\delta_{0}^{2}\over
6\alpha_{i}e}\right)\right)^{2},$ (31)

after some moment with $\delta_{0}:=(\alpha_{i}\delta
e^{-\alpha_{i}t_{c}})^{3}$, i.e.

$\displaystyle\left|{}^{v}\tilde{\ell}_{i,j}(t)\right|\leq
e^{-\gamma_{\star}(t-\tau_{\star})}\left|{}^{v}\tilde{\ell}_{i,j}\left(t_{0}+t_{c}+{1\over
6\alpha_{i}}\right)\right|,$ $\displaystyle t\geq t_{0}+t_{c}+{1\over
6\alpha_{i}}.$

######  Proof 5.8.

The first step is to generate a _square_ regression matrix, which may be full
rank at some moment, by introducing linear bounded-input-bounded-output (BIBO)
operators.444 To be precise, though in our case the regression matrix
$\phi_{i}$ defined in (25) is square, it is not full rank, i.e.
$\det\\{\Pi_{Q(t){\bf y}_{i}(t)}\\}=0,\;\forall t\geq 0.$ Therefore, it
implies the necessity of additional dynamic extensions to continue the design.
It is easy to get

$\displaystyle\dot{\aoverbrace[L1R]{\bm{q}_{i}^{e}-\Phi_{i}{}^{v}\bm{\ell}_{i}}}$
$\displaystyle~{}=~{}-\alpha_{i}(\bm{q}_{i}^{e}-\Phi_{i}{}^{v}\bm{\ell}_{i})+\alpha_{i}(\phi_{i}\bm{q}_{i}-\phi_{i}{}^{v}\bm{\ell}_{i})$
$\displaystyle~{}=~{}-\alpha_{i}(\bm{q}_{i}^{e}-\Phi_{i}{}^{v}\bm{\ell}_{i})+\alpha_{i}(\phi_{i}\phi_{i}^{\top}{}^{v}\bm{\ell}_{i}-\phi_{i}{}^{v}\bm{\ell}_{i})$
$\displaystyle~{}=~{}-\alpha_{i}\big{(}\bm{q}_{i}^{e}-\Phi_{i}{}^{v}\bm{\ell}_{i}\big{)},$

where in the second equation we have used (24), and the last we have used the
facts

$\phi_{i}^{\top}=\phi_{i},\quad\phi_{i}\phi_{i}^{\top}=\phi_{i}$

in terms of the definition $\bm{q}_{i}:=\Pi_{Q{\bf y}_{i}}\cdot\bm{\xi}$.
Hence, it yields a group of new extended LREs

$\bm{q}_{i}^{e}=\Phi_{i}{}^{v}\bm{\ell}_{i}+\epsilon_{t},$ (32)

which is known as the Kreisselmeier’s extended LRE [19], recalling that
$\epsilon_{t}$ is an exponentially decaying term. Then, we mix the regressors
(32) to get three decoupled, _scalar_ regressors for each $i\in{\cal N}$, that
is pre-multipying the adjugate matrix $\mbox{adj}\\{\Phi_{i}\\}$ to the both
sides, thus obtaining

$Y_{i,j}=\Delta_{i}{}^{v}\ell_{i,j}+\epsilon_{t},\quad j\in\\{1,2,3\\}$ (33)

with the definitions

$\displaystyle\bm{Y}_{i}:=\mbox{adj}\\{\Phi_{i}\\}\bm{q}_{i}^{e},\quad\Delta_{i}:=\det\\{\Phi_{i}\\}$
(34)

and $\bm{Y}_{i}:=\mbox{col}(Y_{i,1},Y_{i,2},Y_{i,3}).$

The second step of the proof is to show that from (33) we may obtain $3\times
n$ new _scalar_ linear regressors. For convinience, we define
$\bm{\chi}_{i}(0)=\bm{\chi}_{i,0}$. The time derivative of
$(\bm{\chi}_{i}-{}^{v}\bm{\ell}_{i})$ is an LTV system, which is given by

${d\over
dt}[{\bm{\chi}_{i}-{}^{v}\bm{\ell}_{i}}]=-\Delta_{i}^{2}(t)[\chi_{i}-{}^{v}\bm{\ell}_{i}]$

with $\Delta_{i}$ a scalar bounded signal. Its solution is

$\displaystyle\bm{\chi}_{i}(t)-{}^{v}\bm{\ell}_{i}$
$\displaystyle~{}=~{}\exp\bigg{(}-\int_{0}^{t}\Delta_{i}(s)^{2}ds\bigg{)}[\bm{\chi}_{i}(0)-{}^{v}\bm{\ell}_{i}]$
$\displaystyle~{}=~{}\omega_{i}(t)(\bm{\chi}_{i}(0)-{}^{v}\bm{\ell}_{i})$

by noting that the solution to the fourth equation in (28) is given by

$\omega_{i}(t)=\exp\left(-\int_{0}^{t}\Delta_{i}(s)^{2}ds\right).$

It yields

$\bm{\chi}_{i}(t)-\omega_{i}(t)\bm{\chi}_{i,0}=[1-\omega_{i}(t)]{}^{v}\bm{\ell}_{i},$
(35)

in which we underscore that $\bm{\chi}_{i},\omega_{i}$ and $\bm{\chi}_{i,0}$
($t\geq 0$) are all available in observer design. In this way, we get the
scalar LREs (35) involving the “integral” information of $\Delta_{i}$.
Combining (35) and (33), we obtain new linear regressors555The role of the
parameters $k_{\tt I}^{i}$ will be discussed in the sequel.

$\bm{Y}_{i}+k_{\tt
I}^{i}(\chi_{i}-\omega_{i}\bm{\chi}_{i,0})+\epsilon_{t}=\Delta_{i}^{e}\cdot{}^{v}\bm{\ell}_{i},$
(36)

in which ${}^{v}\bm{\ell}_{i}$ are the unknown constant parameters.
Correspondingly, by defining the estimation error

${}^{v}\tilde{\bm{\ell}}_{i}:={}^{v}\hat{\bm{\ell}}_{i}-{}^{v}\bm{\ell}_{i}$

we have666We omit the exponentially decaying term $\epsilon_{t}$ due to the
specific initialization of $q_{i}^{e}$ and $\Phi_{i}$.

${}^{v}\dot{\tilde{\bm{\ell}}}_{i}=-\gamma_{i}(\Delta_{i}^{e})^{2}\cdot{}^{v}\tilde{\bm{\ell}}_{i}.$
(37)

Now we are in position to verify the first claim. Choosing the Lyapunov
function

$V(\bm{\chi}_{i},\omega_{i},{}^{v}\tilde{\bm{\ell}}_{i})={1\over
2}\sum_{i=1}^{n}\left(|{}^{v}\tilde{\bm{\ell}}_{i}|^{2}+|\bm{\chi}_{i}-{}^{v}\bm{\ell}_{i}|^{2}+|\omega_{i}|^{2}\right),$

it yields

$\displaystyle\dot{V}$
$\displaystyle=-\sum_{i=1}^{n}\left[\gamma_{i}(\Delta_{i}^{e})^{2}|{}^{v}\tilde{\bm{\ell}}_{i}|^{2}+\Delta_{i}^{2}|\bm{\chi}_{i}-{}^{v}\bm{\ell}_{i}|^{2}+\Delta_{i}^{2}\omega_{i}^{2}\right]\leq
0.$

Thus, the system is internally stable.

The second claim is verified invoking $\Delta_{i}^{e}$ are scalar signals.
Recalling the dynamics of ${}^{v}\tilde{\bm{\ell}}_{i}$ in (37), the last
claim is equivalent to verify $\Delta_{i}^{e}\in\mbox{PE}$. To simplify the
presentation, we neglect the index $i$ of the IE condition from the $i$-th
feature points. From the IE assumption of $\Pi_{Q{\bf y}_{i}}$, there exist
$t_{0},t_{c},\delta\in\mathbb{R}_{+}$ such that

$\int_{t_{0}}^{t_{0}+t_{c}}\Pi_{Q(s){\bf y}_{i}(s)}\Pi_{Q(s){\bf
y}_{i}(s)}^{\top}ds\geq\delta I_{3}.$

Noting the second equation in (28), we can verify

$\displaystyle\Phi_{i}(t_{0}+t_{c})~{}\succeq{}$
$\displaystyle\alpha_{i}\int_{t_{0}}^{t_{0}+t_{c}}e^{-\alpha_{i}(t_{0}+t_{c}-s)}\phi(s)ds$
$\displaystyle~{}\succeq{}$
$\displaystyle\alpha_{i}e^{-\alpha_{i}t_{c}}\int_{t_{0}}^{t_{0}+t_{c}}\phi(s)ds$
$\displaystyle~{}={}$
$\displaystyle\alpha_{i}e^{-\alpha_{i}t_{c}}\int_{t_{0}}^{t_{0}+t_{c}}\Pi_{Q(s){\bf
y}_{i}(s)}\Pi_{Q(s){\bf y}_{i}(s)}^{\top}ds$ $\displaystyle~{}\succeq{}$
$\displaystyle\alpha_{i}\delta e^{-\alpha_{i}t_{c}}I_{3}.$

It implies

$\Delta(t_{0}+t_{c})\geq\delta_{0}:=(\alpha_{i}\delta
e^{-\alpha_{i}t_{c}})^{3}.$ (38)

Hence, from the continuity of differential equations, in a small neighborhood
of the moment $t_{0}+t_{c}$, the signal $\Delta(t)$ is lower bounded from
zero. Namely, there exist small $\tau>0$ and $\varepsilon>0$ such that

$\displaystyle\footnotesize\int_{t_{c}^{\prime}-\tau}^{t_{c}^{\prime}}\Delta_{i}(s)ds\geq\sqrt{\varepsilon}$
(39) $\displaystyle~{}\implies{}{}{}$
$\displaystyle\int_{0}^{t}\Delta_{i}^{2}(s)ds\geq{1\over\tau}\varepsilon,~{}\forall
t\geq t_{c}^{\prime}$ $\displaystyle~{}\implies{}{}{}$ $\displaystyle
1-\omega_{i}\geq 1-e^{-{1\over\tau}\varepsilon},~{}\forall t\geq
t_{c}^{\prime}$ $\displaystyle~{}\implies{}{}{}$
$\displaystyle\Delta_{i}^{e}=\Delta_{i}+k_{\tt
I}^{i}(1-\omega_{i})\in\mbox{PE}$

with $t_{c}^{\prime}:=t_{0}+t_{c},$ in which we have used the non-negativeness
property $\Delta_{i}(t)\geq 0$, $\forall t\geq 0$ and the Cauchy-Schwarz
inequality for integrals. For the LTV system (37), it is well known that the
global exponential stability (GES) is equivalent to $\Delta_{i}^{e}$ being PE,
which has been verified in (39). Therefore, the error dynamics admits the
exponential convergence (30) under the IE condition of $\phi_{i}$.

At the end, we give an estimate of the convergence speed of the proposed
mapping observer. In terms of the above analysis, we have
$\Delta_{i}(t_{c})\geq\delta_{0}$ with the parameter $\delta_{0}$ defined in
(38). It is clear that if the matrix $\Pi_{Q{\bf y}_{i}}$ is
$(t_{0},t_{c},\delta)$-IE, then it is also $(t_{0},t_{c}+\tau,\delta)$-IE for
any $\tau\geq 0$ according to the definition of IE. In the same way, we obtain

$\displaystyle\Delta_{i}(t_{0}+t_{c}+\tau)$
$\displaystyle~{}\geq~{}\left[\delta
e^{-\alpha_{i}(t_{0}+t_{c}+\tau)}\right]^{3}$
$\displaystyle~{}=~{}\delta_{0}e^{-3\alpha_{i}\tau},\quad\forall\tau\geq 0,$

and

$\Delta_{i}(t)\geq\delta_{0}e^{-3\alpha_{i}\tau},\quad\forall
t\in[t_{0}+t_{c},t_{0}+t_{c}+\tau].$

It is underlined that the parameter $\delta_{0}$ is independent of $\tau$.
From the dynamics of $\omega$, it yields for $t\geq t_{0}+t_{c}+\tau$

$\displaystyle\omega_{i}(t)$
$\displaystyle~{}=~{}\exp\bigg{(}-\int_{0}^{t}\Delta_{i}(s)^{2}ds\bigg{)}$
$\displaystyle~{}\leq~{}\exp\bigg{(}-\int_{t_{0}+t_{c}}^{t_{0}+t_{c}+\tau}\Delta_{i}(s)^{2}ds\bigg{)}$
$\displaystyle~{}\leq~{}\exp\bigg{(}-\delta_{0}^{2}\tau
e^{-6\alpha_{i}\tau}\bigg{)}.$

Then, invoking the definition of $\Delta_{i}^{e}$ we have for $t\geq
t_{0}+t_{c}+\tau$

$\displaystyle\Delta_{i}^{e}(t)$ $\displaystyle~{}\geq~{}k_{\tt
I}^{i}(1-\omega_{i}(t))$ $\displaystyle~{}\geq~{}k_{\tt
I}^{i}\left[1-\exp\bigg{(}-\delta_{0}^{2}\tau
e^{-6\alpha_{i}\tau}\bigg{)}\right],$

where in the first inequality we have used the positive semi-definiteness of
$\Phi(t)$. By investigating the stationary point of the function

$f(\tau)=1-\exp\left(-\delta_{0}^{2}{\tau\over e^{6\alpha_{i}\tau}}\right),$

we have $\nabla f({1\over 6\alpha_{i}})=0$ thus the function getting its
unique minimum at $\tau_{\star}={1\over 6\alpha_{i}}$. Hence,

$\Delta_{i}^{e}(t)\geq k_{\tt I}^{i}\left(1-\exp\left(-{\delta_{0}^{2}\over
6\alpha_{i}e}\right)\right),\quad t\geq t_{0}+t_{c}+\tau_{\star}.$

It is easy to get

$|{}^{v}\tilde{\ell}_{i,j}(t)|\leq
e^{-\gamma_{\star}(t-\tau_{\star})}|{}^{v}\tilde{\ell}_{i,j}(t_{0}+t_{c}+\tau_{\star})|,\quad
t\geq t_{0}+t_{c}+\tau_{\star},$

with $\gamma_{\star}$ defined in (31). It completes the proof.

In the above, we present a GES landmark observer using the extremely weak IE
condition, and give an estimate of its convergence speed. After obtaining
$({}^{v}X,{}^{v}\bm{\ell}_{1},\ldots,{}^{v}\bm{\ell}_{n})$, we indeed achieve
the visual inertial odometry. Compared with VI-SLAM, the odometry from
${}^{v}X$ is highly sensitive to unavoidable measurement noise, and we will
propose an observer in Section 5.2 to complete the task to estimate ${}^{c}X$,
as well as robustifying the design.

######  Remark 5.9.

Some remarks are made about Theorem 5.7.

  1. (a)

It is interesting to note that the proposed observer for _each landmark_ is
decoupled, thus having computation complexity ${\cal O}(n)$ with $n$
landmarks. This should be compared with ${\cal O}(n^{2})$ for standard EKF-
SLAM algorithms [32].

  2. (b)

All functions in the observer are ${\cal C}^{\infty}$. This is unlike the
approaches via homogeneous domain (with fractional power), where non-smooth or
discontinuous terms are utilized [35].

  3. (c)

The success of the proposed landmark observer design relies on generation of
the new scalar LREs (36), which satisfy the PE condition [27]. It combines two
parts, namely, the first contains information in the current small “interval”
invoking the K-ELRE generated by the filter—the first four equations in
(28)—and the second one consists of historical data using an integral
operation. The parameters $k_{\tt I}^{i}$ are adopted to play a role of
weighting between them.

$\displaystyle\small\underbrace{\bm{Y}_{i}(t)}_{\mbox{\footnotesize``current
interval''}}+~{}\underbrace{k_{\tt
I}^{i}\bm{Y}_{i}^{e}(t)}_{\footnotesize\mbox{historical
information}}~{}=~{}\Delta_{i}^{e}(t){}^{v}\bm{\ell}_{i}.$

######  Remark 5.10.

The odometry ${}^{v}X$ generated from (19) can be viewed as a _fragile_
estimate of the pose $X$. Namely, if an anchor pose is selected in advance,
i.e., pre-defining777 It is standard practice for SLAM algorithms to give an
anchor $X(0)$ in advance, since it is widely recognized that the system
(11)-(12) and (14) is not strongly differentially observable [4]. The
underlying reason is clear that it is ambiguous to identify the origin of
$\\{{\cal I}\\}$ with only measurements in the $\\{{\cal B}\\}$, see for
example [43] in which the estimation error converges to a _quotient manifold_
rather than an isolated equilibrium.  $X(0)=X_{0}:={\cal T}(R_{0},{\bf
x}_{0})\in SE(3)$ in $\\{{\cal I}\\}$, then for (19) by choosing particular
initial conditions we have the invariance

$\small\Big{[}\bm{\xi}(0)={\bf
x}_{0},~{}Q(0)=R_{0}\Big{]}~{}\implies~{}\Big{[}X(t)={}^{v}X(t),~{}\forall
t\geq 0\Big{]}$

under _ideal circumstance_. This, however, is not practically applicable
especially after a long period, since the odometry—the dynamic extension (19)
relies on open-loop integral, which may be a problematic operation yielding
_error accumulation_.

###  5.2 Landmark and Pose Observer in the Inertial Frame

In this section, we introduce a localization and mapping observer, which is
cascaded to the mapping observer in the dynamic extension frame in Theorem 5.7
in order to express all estimates in the inertial frame, and is robust _vis-à-
vis_ measurement noise. That is to synchronize $\hat{X}$ to $X$, equivalently
estimating the transformation ${}^{c}X$.

We now propose the main results of the paper—a localization and mapping
observer to fuse the estimates of landmarks and pose together, which provides
robust estimation $\hat{\bf x},\hat{R}$ and $\hat{\bm{\ell}}_{i}$ in the
inertial frame.

Our design is motivated by the standard EKF-SLAM [12], i.e., making the
landmark estimates $\hat{\bm{\ell}}_{i}$ and the pose estimate $\hat{X}$
“coupled” together.888In EKF-SLAM, the Jacobian of the output function
achieves such coupling between $X$ and $\hat{\bm{\ell}}_{i}$. In this way, the
observation performance of $\hat{\bm{\ell}}_{i}$ and $\hat{X}$ can be
_improved from each other_. To this end, we rely on the following technical
route:

  * -

Estimate the landmarks (denoted as $\bar{\bm{\ell}}_{i}$) in the inertial
frame, using the information of measurement ${\bf y},{\bf u}$, and ${}^{v}X$,
as well as the anchor $(R_{0},{\bf x}_{0})$—see Eq. (41);

  * -

Use the “observation error” between $\bar{\bm{\ell}}_{i}$ and
${}^{v}\bm{\ell}_{i}$ to estimate the transformation ${}^{c}Q$; see the first
equation in (40);

  * -

Express all estimates in the inertial frame, with the estimation convergence
$\hat{X}={\cal T}(\hat{R},\hat{\bf x})\to X$ and
$\hat{\bm{\ell}}_{i}\to\bm{\ell}_{i}$ as $t\to\infty$; see Theorem 5.11.

Localization and mapping observer:

$\left\\{~{}\begin{aligned} {}^{c}\dot{\hat{Q}}&~{}=~{}-(\bm{w}_{\tt
vis})_{\times}{}^{c}\hat{Q}\\\ \dot{\hat{\bf x}}&~{}=~{}\hat{R}{\bf
v}+\sum_{i\in{\cal N}}\sigma_{i}\big{[}\bar{\bm{\ell}}_{i}-\hat{\bf
x}-{}^{c}\hat{Q}^{\top}(\hat{\bm{\ell}}_{i}^{v}-\bm{\xi})\big{]},\end{aligned}\right.$
(40)

with

$\displaystyle\dot{\eta}_{\Sigma_{i}}$
$\displaystyle=F_{\Sigma_{i}}\Big{(}\eta_{\Sigma},\phi_{i}^{\top}(\bm{\xi}-\bm{\xi}(0)+Q(0)R_{0}^{\top}{\bf
x}_{0}),R_{0}Q(0)^{\top}\phi_{i}\Big{)}$ (41)
$\displaystyle\bar{\bm{\ell}}_{i}$ $\displaystyle=N_{\Sigma}(\eta_{\Sigma}),$

where the functions $F_{\Sigma},N_{\Sigma}$ defined by the system $\Sigma$ in
(28) and

  * -

the gains: $k_{i},\sigma_{i}>0,~{}\forall i;$

  * -

the variables:

$\displaystyle{}^{v}\hat{\bm{r}}_{i}$
$\displaystyle:={}^{v}\hat{\bm{\ell}}_{i+1}-{}^{v}\hat{\bm{\ell}}_{i},~{}~{}\bm{w}_{\tt
vis}:=\sum_{i=1}^{n-1}k_{i}{}^{v}\hat{{\bm{r}}}_{i}\times({}^{c}\hat{Q}\bar{\bm{r}}_{i})$
$\displaystyle\bar{\bm{r}}_{i}$
$\displaystyle:=\bar{\bm{\ell}}_{i+1}-\bar{\bm{\ell}}_{i}$

  * -

the observer outputs:

$\displaystyle\hat{R}$ $\displaystyle=$
$\displaystyle~{}~{}{}^{c}\hat{Q}^{\top}Q$ (42)
$\displaystyle\hat{\bm{\ell}}_{i}$ $\displaystyle=$
$\displaystyle~{}~{}{}^{c}\hat{Q}^{\top}({}^{v}\hat{\bm{\ell}}_{i}-\bm{\xi})+\hat{\bf
x},\quad i\in{\cal N}.$

∎

The properties of the above observer are given as follows.

######  Theorem 5.11.

(Localization and mapping) Consider the kinematic model (11) under Assumptions
1-2 with the given anchor $(R_{0},{\bf x}_{0})$. The localization and mapping
observer (41), (40) and (42), and the dynamic extension (19) from the initial
condition ${\cal T}(Q(0),\xi(0))$, together with the mapping observer in
Theorem 5.7, achieve the task (16) almost globally.999The qualifier “almost”
stems from the fact that the set of initial conditions which do not guarantee
the convergence has zero Lebesgue measure.

######  Proof 5.12.

The proof is given in Appendix.

The above design provides an approach to deal with drift in odometry. The
discretization of the proposed PEBO-SLAM observer design and its
implementation procedure is summarized in Algorithm 1.

###  5.3 Discussions

Algorithm 1 Visual PEBO-SLAM (discrete time)

0: Discrete-time IMU measurements (velocities ${\bf u}(k)$), bearing
measurements ${\bf y}_{i}(k)$ ($k\in\mathbb{N}$), sampling time $T$ and the
final moment $k_{\tt max}$

0: $\hat{X}(k)$ and $\hat{\bm{\ell}}_{i}(k)$ for all $k$ and $i\in{\cal N}$

1: Initialize the observer: $X(0)={\cal T}(R_{0},{\bf x}_{0})$,
${}^{c}\hat{Q}(0)=I$ and $\hat{\bf x}(0)=\bm{\xi}(0)$.

2: while $k<k_{\tt max}$ do

3: Compute ${}^{v}X$ from the dynamic extension  $\displaystyle\bm{\xi}(k+1)$
$\displaystyle~{}=~{}\bm{\xi}(k)+TQ(k){\bf v}(k)$ $\displaystyle Q(k+1)$
$\displaystyle~{}=~{}Q(k)\exp(T\bm{\Omega}_{\times})$ /*${}^{v}X={\cal
T}(Q,\bm{\xi})$ with ${\cal T}\cal(\cdot)$ defined in (1)*/

4: Run the gradient estimator to identify ${}^{v}\hat{\bm{\ell}}_{i}$

5: if the $i$-th landmark is visible then

6: update the estimate ${}^{v}\hat{\bm{\ell}}_{i}$ as
$\displaystyle{}^{v}\hat{\bm{\ell}}_{i}(k+1)=$
$\displaystyle{}^{v}\hat{\bm{\ell}}_{i}(k)+{1\over\gamma+1}\left[\bm{q}_{i}(k+1)-\phi_{i}(k+1){}^{v}\hat{\bm{\ell}}_{i}^{v}(k)\right]$
$\phi_{i}:=\Pi_{Q{\bf y}_{i}}^{\top}$, $\bm{q}_{i}:=\Pi_{Q{\bf
y}_{i}}\bm{\xi}$ /*$\alpha_{i}>0$ for all $i\in{\cal N}$*/ /*may be replaced
by the DREM estimator (28)*/

7: else if it is invisible then

8: update it as ${}^{v}\hat{\bm{\ell}}_{i}(k)={}^{v}\hat{\bm{\ell}}_{i}(k-1)$

9: end if

10: Run the localization observer  $\displaystyle{}^{c}\hat{Q}(k+1)$
$\displaystyle=\exp(-T\cdot S(\bm{w}_{\tt vis})){}^{c}\hat{Q}(k)$
$\displaystyle{\hat{\bf x}}(k+1)$ $\displaystyle=\hat{\bf
x}(k)+T\left[\hat{R}(k){\bf v}(k)+\sum_{i=1}^{n}\sigma_{i}\big{(}{\hat{\bf
z}_{i}}(k)-\hat{\bm{\ell}}_{i}(k)\big{)}\right]$ $\displaystyle\hat{R}({k})$
$\displaystyle={}^{c}\hat{Q}^{\top}(k)Q(k)$
$\displaystyle\hat{\bm{\ell}}_{i}(k)$
$\displaystyle={}^{c}\hat{Q}^{\top}(k)({}^{v}\hat{\bm{\ell}}_{i}(k)-\bm{\xi}(k)){+}\hat{\bf
x}(k)$ with the functions  $\displaystyle\bm{w}_{\tt vis}$
$\displaystyle~{}=~{}\left\\{\begin{aligned}
&\sum_{i=1}^{n-1}k_{i}\hat{{\bm{r}}}_{i}^{v}{\times}({}^{c}\hat{Q}\bar{\bm{r}}_{i})&\mbox{if~{}}n>2\\\
&0&\mbox{otherwise}\end{aligned}\right.$ $\displaystyle{}^{v}\hat{\bm{r}}_{i}$
$\displaystyle~{}=~{}{}^{v}\hat{\bm{\ell}}_{i+1}-{}^{v}\hat{\bm{\ell}}_{i},\quad\bar{\bm{r}}_{i}=\bar{\bm{\ell}}_{i+1}-\bar{\bm{\ell}}_{i}.$
/*for all $i\in{\cal N}$*/

11: Run the discretized gradient estimator
$\displaystyle\bar{\bm{\ell}}_{i}(k+1)$
$\displaystyle~{}=~{}\bar{\bm{\ell}}_{i}(k)+{1\over\gamma+1}\left[\bm{q}_{x,i}(k+1)-\hat{\phi}_{i}(k+1)\bar{\bm{\ell}}_{i}(k)\right]$
$\displaystyle\bm{q}_{x,i}(k)$ $\displaystyle~{}=~{}\hat{\phi}_{i}(k){\hat{\bf
x}}(k),\quad\hat{\phi}_{i}(k)=\left\\{\begin{aligned} &\Pi_{\hat{R}(k){\bf
y}_{i}(k)}&&\mbox{visible}\\\ &0_{3\times
3}&&\mbox{invisible}\end{aligned}\right.$

12: $\hat{R}(k)={}^{c}\hat{Q}^{\top}(k)Q(k)$

13:
$\hat{\bm{\ell}}_{i}(k)={}^{c}\hat{Q}^{\top}(k)({}^{v}\hat{\bm{\ell}}_{i}(k)-\bm{\xi}(k))+\hat{\bf
x}(k)$ /*for all $i\in{\cal N}$*/

14: $k=k+1$

15: end while

16: return $\hat{X}={\cal T}(\hat{R},\hat{\bf x}),~{}\hat{\bm{\ell}}_{i}$ for
all $k\in\mathbb{N}$ and $i\in{\cal N}$.

In this section we give some remarks to the proposed design.

######  Remark 5.13.

Note that in the estimator (41) the predefined anchor is involved, rather than
the information of $\hat{X}$. In the standard SLAM formulation, it is natural
to impose the constraint of initializing the estimate from the anchor
$\hat{R}(0)=R_{0},~{}\hat{\bf x}(0)={\bf x}_{0}.$ Then, in (41) we may replace
the “anchor” $(R_{0},{\bf x}_{0})$ by its estimate $(\hat{R}(t),\hat{\bf
x}(t))$ at the _current_ moment $t$; correspondingly $\xi(0)$ and $Q(0)$ are
replaced by $Q(t)$ and $R(t)$. Namely, $\bar{\bm{\ell}}_{i}$ is generated
by101010Roughly speaking, it can be understood that the anchor and the initial
moment are defined in a “moving manner”. The conclusion still holds true since
the observer for $\hat{X}={\cal T}(\hat{R},\hat{\bf x})$ is asymptotically
stable, by invoking that asymptotic stability implies the invariance
$[\hat{R}(0)=R_{0},\hat{\bf x}(0)={\bf
x}_{0}]\implies[\hat{R}(t)=R(t),\hat{\bf x}(t)={\bf x}(t)]$ for all $t\geq 0$.

$\displaystyle\dot{\eta}_{\Sigma}$
$\displaystyle=F_{\Sigma}\Big{(}\eta_{\Sigma},\phi_{i}^{\top}(Q\hat{R}^{\top}\hat{\bf
x}),\hat{R}Q^{\top}\phi_{i}\Big{)}$ (43) $\displaystyle\bar{\bm{\ell}}_{i}$
$\displaystyle=N_{\Sigma}(\eta_{\Sigma}),$

In this way, the landmark estimate $\hat{\bm{\ell}}_{i}$ in $\\{{\cal I}\\}$
and the pose estimate $(\hat{R},\hat{\bf x})$ are in a “feedback close-loop”.

######  Remark 5.14.

From the fourth item of Theorem 5.7, we note that the convergence rate is
proportional to the gain $\gamma_{i}$ after the moment
$(t_{0}+t_{c}+\tau_{\star})$. The fact that we need to wait a moment to tune
the observer is intuitive, since the $i$-th feature point gets sufficient
excitation after $t_{0}+t_{c}$. Due to favorable decoupling property, we may
easily tune the convergence rate of each element ${}^{v}\tilde{\ell}_{i,j}$
separately.

######  Remark 5.15.

As illustrated before, the SLAM problem is concerned with two tasks, i.e.,
mapping and localization. If we are only interested in mapping, Theorem 5.7,
indeed, provides a feasible solution to it. A similar problem has been
comprehensively studied in [21] via transforming the system into an LTV
system, and then solved by LTV Kalman filter, in which the pose estimation is
not involved, and the landmark coordinates ${}^{{\cal B}}\bm{\ell}_{i}$ in the
body frame $\\{{\cal B}\\}$ are selected as the system state. In that case,
the landmark coordinates ${{\ell_{i}}}^{{\cal B}}$ keep “rotating” when the
algorithm is running. In contrast, a favorable property of the landmark
observer in Theorem 5.7 relies on that the landmark coordinates
${}^{v}\bm{\ell}_{i}$ are _stationary_.

######  Remark 5.16.

Algorithm 1 is applicable to the two-dimensional case with slight
modifications. For this case, ${\bf
v}\in\mathbb{R}^{2},\bm{\Omega}\in\mathbb{R},{\bf x}\in\mathbb{R}^{2}$ and
$R\in SO(2)$. The algorithm is modified by defining the operation
$\mathbf{a}\times\mathbf{b}=a_{1}b_{2}-a_{2}b_{1}$ for
$\mathbf{a},\mathbf{b}\in\mathbb{R}^{2}$, and the operator
$\bm{\Omega}_{\times}=\begin{bmatrix}0&-\bm{\Omega}\\\
\bm{\Omega}&0\end{bmatrix}.$

######  Remark 5.17.

In stability analysis of the localization observer, we require that at least
three feature points visible at all the time to guarantee the asymptotic
convergence of the attitude estimate. If this cannot be verified in some
intervals, we may also involve the feature points in recent past for the
function $\bm{\omega}_{\tt vis}$, since both ${\bm{r}}_{i}$ and
${}^{v}{\bm{r}}_{i}$ are constant vectors.

######  Remark 5.18.

In the proposed observer, we utilize the vectors $\bar{\bm{r}}_{i}$ and their
corresponding estimates $\hat{\bm{r}}_{i}$ to generate the error correction
term, as illustrated in (40)-(42). Specially, we define the vectors as
$\bar{\bm{r}}_{i}:=\bar{\bm{\ell}}_{i+1}-\bar{\bm{\ell}}_{i}$, which represent
a “minimal” construction ensuring convergence conditions as proven in our
analysis. However, it is worth noting that alternative approaches can be
explored for selecting these vectors among landmarks. One such approach
involves considering a “fully connected graph” among all visible landmarks,
which could potentially enhance robustness but at the expense of significantly
increased computational burden, growing quadratically. Further investigation
into the construction of these vectors among landmarks would be valuable and
warrants future research.

##  6 Simulations

In this section, we give some simulation results to demonstrate effectiveness
of the proposed observer, considering the cases with and without sufficient
excitation.

###  6.1 With sufficient excitation

We first consider a scenario that the robot keeps moving, such that the PE
condition is satisfied for all feature points. A robot is simulated from the
point ${\bf x}(0)=[1,1,2]^{\top}$ and the initial attitude
$R(0)=[\cos({\pi\over 6}),-\sin({\pi\over 6}),0;\sin({\pi\over
6}),\cos({\pi\over 6}),0\\\ 0,0,1]$ with the velocity ${\bf
v}(t)=[1,0,0]^{\top}$ and $\bm{\Omega}(t)=[0,0,-0.4]^{\top}$; see Figs. 4(a)
and 4(c) for the pose trajectory.

(a) Robot trajectory ${\bf x}$ and its estimate

(b) Pose estimation error $\tilde{\bf x}$

(c) Attitude $R$ and its estimate

(d) Attitude estimation error $\tilde{R}$

Figure 4: Simulation results with the robot trajectory satisfying PE

We consider six landmarks in simulation, and assume that they are measured all
the time. The initial conditions are set as

$\small Q(0)=\begin{bmatrix}\cos({\pi\over 2})&-\sin({\pi\over 2})&0\\\
\sin({\pi\over 2})&\cos({\pi\over 2})&0\\\
0&0&1\end{bmatrix},~{}{}^{c}\hat{Q}(0)=I_{3},~{}\bm{\xi}(0)=\begin{bmatrix}0\\\
1\\\ 1\end{bmatrix},$

and $\hat{\bf x}(0)=0_{3\times 1}$. The observer gains are selected as
$\alpha_{i}=5,\gamma_{i}=100,k_{\tt I}^{i}=5$ for $i\in{\cal N}$, and
$\rho_{j}=1$ for $j=1,2,3$. The simulation was done in Matlab/Simulink with
the fixed step size 0.001 seconds. To evaluate robustness of the observer, as
well as to make simulations more realistic, we added noise to the measured
velocities and the bearing.

First we show the simulation results of the mapping observer in the dynamic
extension frame in Fig. 5(a), verifying the theoretical results in Theorem
5.7, in particular, the element-wise monotonicity providing favorable
performance. This property is also illustrated by Fig. 5(d), which shows the
evolution of each element ${}^{v}\tilde{\ell}_{i,j}$ the estimation error for
the second landmark. Then, we evaluate the performance of the localization
observer in Theorem 5.11 with the simulation results given in Figs. 4, which
illustrates the robot pose $(R,{\bf x})$ and its estimate.

(a) Landmark estimates ${}^{v}\hat{\bm{\ell}}_{i}$ in $\\{{\cal V}\\}$

(b) Landmark estimates $\hat{\bm{\ell}}_{i}$ in $\\{{\cal I}\\}$

(c) Landmark estimation error in $\\{{\cal V}\\}$

(d) Error for the 2nd landmark

Figure 5: Feature points estimation under the PE condition

Note that we adopt the metric $|\tilde{R}|_{I}$ with
$\tilde{R}:=R\hat{R}^{\top}$ to characterize the attitude estimation error
with the definition $|\tilde{R}|_{I}^{2}:={1\over
4}\mbox{tr}(I_{3}-\tilde{R})$. For a better illustration, we visualize in Fig.
4(c) presenting the attitude trajectory and its estimate by means of the unit
vector along $x$-axis, i.e., $e_{1}=[1,0,0]^{\top}$, of the body-fixed frame,
which is plotted in the inertial frame. Note that the attitude can be uniquely
determined in terms of right-handed coordinate. As expected, after a short
transient stage the pose estimate asymptotically converges to its true value
with a small ultimate error, which is caused by measurement noise. It also
shows the robustness of the landmark observer w.r.t. high-frequency
measurement noise. The estimate of the landmark coordinates in the inertial
frame $\\{{\cal I}\\}$ are given in Fig. 5(b).

(a) The effect to $|{}^{v}\tilde{\bm{\ell}}|$

(b) The effect to $\tilde{R}$

Figure 6: The effect of parameter $\gamma$ to the convergence rate

At the end, we show the effect of the gain parameters $\gamma_{i}$ to the
convergence rates for the estimates ${}^{v}\hat{\bm{\ell}}$ and $\hat{R}$. In
Fig. 6(a), it verifies the theoretical analysis in the fourth term of Theorem
5.7, i.e., a larger gain implying a faster convergence rate. However, it
cannot be arbitrarily fast since we need to wait a while to obtain sufficient
excitation. On the other hand, the estimation errors
${}^{v}\tilde{\bm{\ell}}_{i}$ appear in the dynamics of $\tilde{R}$, and thus
tuning $\gamma_{i}$ also affects the convergence rate of $\tilde{R}$.

###  6.2 Not satisfying the PE condition

To illustrate the merits of the proposed design, in this section we consider
robot trajectories without sufficient excitation. Specifically, all the
scenarios are adopted from that in Section 5.1 but with a different trajectory
of the robot. In this case, we assume that the robot stopped at $t=12$s with
velocity

$\footnotesize{\bf v}=\left\\{\begin{aligned} {}&[1,0,0]^{\top}&t\in[0,12]\\\
&0_{3\times 1},&t\geq
12\end{aligned}\right.,\quad\bm{\Omega}=\left\\{\begin{aligned}
{}&[0,0,-0.4]^{\top},&t\in[0,12]\\\ &0_{3\times 1},&t\geq
12\end{aligned}\right..$

It guarantees that all the landmarks satisfy the IE condition with $t_{0}=0$
and $t_{c}=12$s, but not guaranteeing PE. The observer gains are selected as
$\alpha_{i}=5,\gamma_{i}=100,k_{\tt I}^{i}=20$ for $i\in{\cal N}$, and
$\rho_{j}=1$ for $j=1,2,3$. From Figs. 7-8 we observe that the pose estimation
has a satisfactory performance, even when the PE condition are not verified.
The pose trajectory is given in Figs. 7(a) and 7(c). We underline that this
scenario can be considered mathematically equivalent to the situation where
the landmarks become invisible after 12 seconds, after which the excitation
condition does not hold anymore. Indeed, this is more common in various SLAM
problems.

(a) Robot trajectory $\hat{\bf x}$ and its estimate

(b) Pose estimation error $\tilde{\bf x}$

(c) Attitude $R$ and its estimate

(d) Attitude estimation error $\tilde{R}$

Figure 7: Simulation results with the robot trajectory not satisfying PE

(a) ${}^{v}\hat{\bm{\ell}}_{i}$ in the dynamic extension frame

(b) $\hat{\bm{\ell}}_{i}$ in the inertial frame

Figure 8: Landmark estimates in the dynamic extension and the inertial frames
without PE

We finally compare the proposed design to the robo-centric SLAM observer in
[21], which requires the system trajectory being UCO. Indeed, the observer in
[21] studies the case that the feature coordinates are expressed in the body-
fixed frame $\\{{\cal B}\\}$, i.e., ${}^{{\cal
B}}\bm{\ell}_{i}=R^{\top}\left(\bm{\ell}_{i}-{\bf x}\right),$ which are time-
varying signals. In order to make a fair comparison, we plot the evolution of
the _norms_ of the observation errors, i.e., $|\tilde{\bm{\ell}}_{i}|$ of the
proposed design and $|{}^{{\cal B}}\tilde{\bm{\ell}}_{i}|$ for the one in
[21], since rotation does not affect norms. As shown in Fig. 9, the proposed
observer guarantees the estimates converging to some relatively small
neighbourhoods of their true values in the presence of only the IE condition,
showing good robustness against noise. In contrast, Fig. 9(b) shows that the
estimates from the observer in [21] stop converging at the moment
$t_{0}+t_{c}=12$s with large steady-state errors, since the UCO condition
required in LTV Kalman filter cannot be satisfied. It is interesting to note
that the estimates diverge from that moment due to the accumulation of noise,
which is conspicuous by its absence in our design.

(a) Landmark estimation errors $|\tilde{\bm{\ell}}_{i}|$ of the proposed
design in the inertial frame

(b) Landmark estimation errors $|{}^{{\cal B}}\tilde{\bm{\ell}}_{i}|$ of the
LTV Kalman filter [21] in the body-fixed frame

Figure 9: Comparison between the proposed observer and the LTV Kalman filter
in [21]

##  7 Conclusion

In this paper we have introduced a novel visual inertial SLAM observer
framework called PEBO-SLAM. A key observation is that the coordinates of
feature points in the dynamic extension frame are constant, based on which we
are able to get a set of linear regression models, and then transform the
problem into online parameter estimation. To address this, we extend the PEBO
methodology to the manifold $SE(3)\times\mathbb{R}^{3n}$, the unknown
“parameters” in our context being the landmark coordinates
${}^{v}\bm{\ell}_{i}$ together with the constant relative rigid transformation
${}^{c}X$. A simple constructive solution is also provided, with guaranteed
almost global asymptotic stability, while significantly relaxing the strong PE
or UCO-type conditions required in the existing literature. As future work, it
is of practical interests to study the performance limitation from gyro noise
and bias, as well as how to robustify the proposed observer. The final remark
is that, as a byproduct, the proposed method can be also used as an
initialization algorithm due to the globality and convexity.

##  8 Acknowledgement

Partial work has been done when the second author was with I3S-CNRS, France.
He appreciates the fruitful discussions with Prof. Tarek Hamel.

###  .1 Proof of Theorem 5.11

The proof consists of two parts: 1) illustrating the exponential convergence
of $|\bar{\bm{\ell}}_{i}-\bm{\ell}_{i}|\to 0$ as $t\to\infty$, which will be
used to synchronize $\hat{X}$ to $X$; and 2) showing that $\hat{\ell}_{i}$,
$\hat{\bf x}$ and $\hat{R}$ converges to their true values in the inertial
frame.

Part 1. Since the anchor is fixed at $X(0)={\cal T}(R_{0},{\bf x}_{0})$,
recalling (24) it is easy to get the LREs

$\displaystyle\quad\bm{q}_{i}=\Pi_{Q{\bf y}_{i}}{}^{v}\bm{\ell}_{i}$ (44)
$\displaystyle\overset{\eqref{const:ldmk}}{\implies}$
$\displaystyle\quad\bm{q}_{i}=\Pi_{Q{\bf
y}_{i}}\Big{[}{}^{c}\bm{\xi}+{}^{c}Q\bm{\ell}_{i}\Big{]},$
$\displaystyle\overset{\eqref{id:1}}{\implies}$
$\displaystyle\quad\bm{q}_{i}=\Pi_{Q{\bf
y}_{i}}\Big{[}\bm{\xi}(0)-Q(0)R_{0}^{\top}{\bf
x}_{0}+Q(0)R_{0}^{\top}\bm{\ell}_{i}\Big{]}.$

Then, we have the linear regression model

$\phi_{i}^{\top}(\bm{\xi}-\bm{\xi}(0)+Q(0)R_{0}^{\top}{\bf
x}_{0})=[R_{0}Q(0)^{\top}\phi_{i}]^{\top}\bm{\ell}_{i}.$

It is straightforward to verify

$\phi_{i}\in\mbox{~{}PE}\quad\implies\quad[R_{0}Q(0)^{\top}\phi_{i}]\in\mbox{~{}PE}$

by recalling the full-rankness of $R_{0}$ and $Q(0)$. Using the same arguments
in Theorem 5.7, we able to show that for $i\in{\cal N}$

$\lim_{t\to\infty}|\bar{\bm{\ell}}_{i}(t)-\bm{\ell}_{i}|=0\quad\mbox{(exp.)}.$
(45)

Hence, the variables ($\bar{\bm{\ell}}_{i}$)s can be viewed as “known” vectors
in $\\{{\cal I}\\}$. Then, we may generate an error between
$\bar{\bm{\ell}}_{i}$ and ${}^{v}\hat{\bm{\ell}}_{i}$ to synchronize $\hat{X}$
to $X$ asymptotically, and thus we are able to get the estimates of all
landmarks in the inertial frame. To be precise, we need to show the asymptotic
convergence of ${}^{c}\hat{X}={\cal T}({}^{c}\hat{Q},{}^{c}\hat{\bm{\xi}})\in
SE(3)$ to its true value ${}^{c}X$ defined in Lemma 1, which is sufficient to
show $\hat{X}\to X$ in terms of the identity (21) for the proposed PEBO.

Part 2. Define the estimation error of ${}^{c}Q$ as

${}^{c}\tilde{Q}:={}^{c}Q{}^{c}\hat{Q}^{\top},$

and then

$\displaystyle{}^{c}\dot{\tilde{Q}}={}^{c}Q{}^{c}\dot{\hat{Q}}^{\top}={}^{c}\tilde{Q}(\bm{w}_{\tt
vis})_{\times}.$ (46)

We also define a _nominal_ correction term

$\bm{w}_{\tt
vis}^{*}=\sum_{i=1}^{n-1}k_{i}{\bm{r}}_{i}^{v}\times({}^{c}\hat{Q}{\bm{r}}_{i}),$
(47)

and we will show that the difference $|\bm{w}_{\tt vis}^{*}-\bm{w}_{\tt
vis}|\to 0$ exponentially as $t\to 0$. It is easy to see

$\displaystyle(\bm{w}_{\tt vis}^{*})_{\times}$ $\displaystyle~{}=~{}{1\over
2}\sum_{i=1}^{n-1}k_{i}\left[{}^{c}\hat{Q}{\bm{r}}_{i}({}^{v}{\bm{r}}_{i})^{\top}-{}^{v}{\bm{r}}_{i}({}^{c}\hat{Q}{\bm{r}}_{i})^{\top}\right]$
$\displaystyle~{}=~{}\sum_{i=1}^{n-1}k_{i}\mbox{skew}\left({}^{c}\hat{Q}{\bm{r}}_{i}{\bm{r}}_{i}^{\top}{}^{c}Q^{\top}\right)$
$\displaystyle~{}=~{}\sum_{i=1}^{n-1}k_{i}\mbox{skew}\left({}^{c}\tilde{Q}^{\top}{}^{c}Q{\bm{r}}_{i}{\bm{r}}_{i}^{\top}{}^{c}Q^{\top}\right)$
$\displaystyle~{}=~{}\mbox{skew}\left({}^{c}\tilde{Q}^{\top}M\right),$

with
$M:=\sum_{i}^{n-1}k_{i}{}^{c}Q{\bm{r}}_{i}{{\bm{r}}_{i}^{\top}}{}^{c}Q^{\top}$,
and in the second equation we have used

$\displaystyle{}^{v}{\bm{r}}_{i}$
$\displaystyle={}^{v}\bm{\ell}_{i+1}-{}^{v}\bm{\ell}_{i}$
$\displaystyle={}^{c}\bm{\xi}+{}^{c}Q\bm{\ell}_{i+1}-({}^{c}\bm{\xi}+{}^{c}Q\bm{\ell}_{i})$
$\displaystyle={}^{c}Q{\bm{r}}_{i}.$

We underscore that $M$ is a constant symmetric matrix, which is positive
semidefinite for $n_{\ell}=3$, and is positive definite for $n_{\ell}>3$ due
to Assumption 2.

On the other hand, since111111With a slight abuse of notation, we use
$\epsilon_{t}$ to represent some exponentially decaying generally, and do not
distinguish which specific signals they refer to in order to simplify
presentation.

$\displaystyle{}^{v}\hat{\bm{r}}_{i}$
$\displaystyle~{}=~{}{}^{v}\bm{\ell}_{i+1}-{}^{v}\bm{\ell}_{i}+{}^{v}\tilde{\bm{\ell}}_{i+1}(t)-{}^{v}\tilde{\bm{\ell}}_{i}(t)$
$\displaystyle~{}=~{}{}^{v}{\bm{r}}_{i}+\epsilon_{t}$

and $\bar{\bm{r}}_{i}={\bm{r}}_{i}+\epsilon_{t}$, invoking the compactness on
$SO(3)$ we have

$\bm{w}_{\tt vis}=\bm{w}_{\tt vis}^{*}+\epsilon_{t}.$ (48)

From (46) and (48), we have

${}^{c}\dot{\tilde{Q}}={}^{c}\tilde{Q}\mbox{skew}\left({}^{c}\tilde{Q}^{\top}M\right)+{\cal
E}(t)=:f_{q}({}^{c}\tilde{Q},t),$ (49)

in which ${\cal E}(t)$ represents some exponentially decaying signal, invoking
compactness of the space which ${}^{c}\tilde{Q}$ lives in. Then, we have
$|\mbox{tr}({\cal E}^{\top}(t)M)|\leq a_{0}e^{-a_{1}t}=:\varepsilon(t),$ for
some $a_{0},a_{1}>0$. Similarly to the proof of [37, Prop. 6], we consider the
time-varying Lyapunov function candidate

$V({}^{c}\tilde{Q},t)=\sum_{j=1}^{n_{\ell}-1}k_{i}|{\bm{r}}_{i}|^{2}-\mbox{tr}({}^{c}\tilde{Q}^{\top}M)+\int_{t}^{\infty}|\varepsilon(s)|ds,$

which is well posed since $\varepsilon$ is absolutely integrable. Its time
derivative satisfies
$\dot{V}\leq-\big{\|}\mbox{skew}\big{(}{}^{c}\tilde{Q}^{\top}M\big{)}\big{\|}^{2}$.
Then, it yields
$\int_{0}^{\infty}\big{\|}\mbox{skew}\big{(}{}^{c}\tilde{Q}^{\top}(t)M\big{)}\big{\|}^{2}dt<+\infty.$
Using Babalat’s lemma, we conclude that all the trajectories converge to the
invariant set $\Omega_{e}:=\\{{}^{c}\tilde{Q}\in
SO(3)~{}|~{}\mbox{skew}\big{(}{}^{c}\tilde{Q}^{\top}M\big{)}=0\\},$ which can
be shown containing a locally exponentially stable equilibrium
${}^{c}\tilde{Q}=I_{3}$, and other three isolated unstable equilibria
$\mathbb{Q}_{i}$ ($i=1,2,3$) via a similar procedure of the proof in [23, Thm.
5.1]. From the non-autonomous version of Hartman-Grobman theorem [2], the
dynamics (49) is topologically equivalent to an LTV dynamics in a small
neighborhood of these three unstable equilibria. As a result, only some very
specific trajectories, from a zero Lebesgue measure set ${\cal M}_{\epsilon}$,
ultimately converge to the unstable equilibria, thus yielding the almost GAS
of the error dynamics (49). As a result,
$\lim_{t\to\infty}\|\hat{R}(t)-R(t)\|=0$ with $\hat{R}\in SO(3)$.

After verifying convergence of the attitude estimate $\hat{R}(t)$, we then
need to study the position estimate $\hat{\bf x}$. To address this, let us
define the error vector of ${\bf x}$ as $\tilde{\bf x}:=\hat{\bf x}-{\bf x}$,
and then

$\displaystyle\dot{\tilde{\bf x}}$ $\displaystyle~{}=~{}(\hat{R}-R){\bf
v}+\sum_{i=1}^{n-1}\sigma_{i}\big{(}\bar{\bm{\ell}}_{i}-\hat{\bf
x}-{}^{c}\hat{Q}^{\top}(\ell_{i}^{v}-\bm{\xi})+\epsilon_{t}\big{)}$
$\displaystyle~{}=~{}(\hat{R}-R){\bf
v}+\sum_{i=1}^{n-1}\sigma_{i}\big{(}\bm{\ell}_{i}-\hat{\bf
x}-{}^{c}\hat{Q}^{\top}QR^{\top}(\bm{\ell}_{i}-{\bf x})+\epsilon_{t}\big{)}$
$\displaystyle~{}=~{}(\hat{R}-R){\bf
v}+\sum_{i=1}^{n-1}\sigma_{i}\big{(}\bm{\ell}_{i}-\hat{\bf
x}-\tilde{R}^{\top}(\bm{\ell}_{i}-{\bf x})+\epsilon_{t}\big{)}$
$\displaystyle~{}=~{}-\sum_{i=1}^{n-1}\sigma_{i}\tilde{\bf x}+(\hat{R}-R){\bf
v}+\sum_{i=1}^{n-1}\sigma_{i}\big{(}(I-\tilde{R}^{\top})(\bm{\ell}_{i}-\hat{\bf
x})\big{)}$ $\displaystyle~{}:=~{}-\sum_{i=1}^{n-1}\sigma_{i}\tilde{\bf
x}+\Delta_{\tt p}(t).$

From $\|\hat{R}(t)-R(t)\|\to 0$ and the boundedness of ${\bf v}(t)$, we have
the asymptotic convergence condition $|\Delta_{\tt p}|\to 0$. The dynamics of
$\tilde{\bf x}$ can be viewed as a stable LTI system perturbed by a vanishing
term $\Delta_{\tt p}$, and thus following the standard perturbation analysis
on Euclidean space we have

$\lim_{t\to\infty}|\hat{\bf x}(t)-{\bf x}(t)|=0.$

At the end, we show the landmark estimation $\hat{\bm{\ell}}_{i}$ will
converge to their true values ultimately. Invoking the facts
$\bm{\xi}(t)={}^{c}\bm{\xi}+{}^{c}Q{\bf x}(t)$, we have

$\displaystyle{{\ell_{i}}}$
$\displaystyle~{}=~{}{}^{c}Q^{\top}({}^{v}\bm{\ell}_{i}-{}^{c}\bm{\xi})$
$\displaystyle~{}=~{}{}^{c}Q^{\top}({}^{v}\bm{\ell}_{i}-\bm{\xi}+{}^{c}Q{\bf
x})$ $\displaystyle~{}=~{}{}^{c}Q^{\top}({}^{v}\bm{\ell}_{i}-\bm{\xi})+{\bf
x}.$

Therefore, the landmark estimates $\hat{\bm{\ell}}_{i}$ in (42) guarantee
$\lim_{t\to\infty}|\hat{\bm{\ell}}_{i}(t)-\bm{\ell}_{i}|=0.$ It completes the
proof. $\blacksquare$

## References

  * [1] S. Aranovskiy, A. Bobtsov, R. Ortega, and A. Pyrkin.  Performance enhancement of parameter estimators via dynamic regressor extension and mixing.  IEEE Trans. Autom. Control, 62(7):3546–3550, 2016.
  * [2] B. Aulbach and T. Wanner.  The Hartman—Grobman theorem for Carathéodory-type differential equations in Banach spaces.  Nonlinear Anal. Theory Methods Appl., 40(1-8):91–104, 2000.
  * [3] P. Bernard, V. Andrieu, and D. Astolfi.  Observer design for continuous-time dynamical systems.  Annu. Rev. Control., 53:224–248, 2022.
  * [4] G. Besançon.  Nonlinear Observers and Applications.  Springer, 2007.
  * [5] E. Bjørne, E. F. Brekke, T. H. Bryne, J. Delaune, and T. A. Johansen.  Globally stable velocity estimation using normalized velocity measurement.  Int. J. Robot. Res., 39(1):143–157, 2020.
  * [6] M. Bosse, P. Newman, J. Leonard, and S. Teller.  Simultaneous localization and map building in large-scale cyclic environments using the atlas framework.  Int. J. Robot. Res., 23(12):1113–1139, 2004.
  * [7] C. Cadena, L. Carlone, H. Carrillo, Y. Latif, D. Scaramuzza, J. Neira, I. Reid, and J. J. Leonard.  Past, present, and future of simultaneous localization and mapping: Toward the robust-perception age.  IEEE Trans. Robot., 32(6):1309–1332, 2016.
  * [8] A. De Luca, G. Oriolo, and P. Robuffo Giordano.  Feature depth observation for image-based visual servoing: Theory and experiments.  Int. J. Robot. Res., 27(10):1093–1116, 2008.
  * [9] M. G. Dissanayake, P. Newman, S. Clark, H. F. Durrant-Whyte, and M. Csorba.  A solution to the simultaneous localization and map building (SLAM) problem.  IEEE Trans. Robot. Autom., 17(3):229–241, 2001.
  * [10] B. J. Guerreiro, P. Batista, C. Silvestre, and P. Oliveira.  Globally asymptotically stable sensor-based simultaneous localization and mapping.  IEEE Trans. Robot., 29(6):1380–1395, 2013.
  * [11] T. Hamel and C. Samson.  Position estimation from direction or range measurements.  Automatica, 82:137–144, 2017.
  * [12] S. Huang and G. Dissanayake.  Convergence and consistency analysis for extended Kalman filter based SLAM.  IEEE Trans. Robot., 23(5):1036–1049, 2007.
  * [13] S. Huang, Y. Lai, U. Frese, and G. Dissanayake.  How far is slam from a linear least squares problem?  In IEEE/RSJ Int. Conf. Intell. Robot. Syst., pages 3011–3016. IEEE, 2010.
  * [14] V. Ila, L. Polok, M. Solony, and P. Svoboda.  SLAM++: A highly efficient and temporally scalable incremental SLAM framework.  Int. J. Robot. Res., 36(2):210–230, 2017.
  * [15] V. Ila, J. M. Porta, and J. Andrade-Cetto.  Information-based compact pose SLAM.  IEEE Trans. Robot., 26(1):78–93, 2009.
  * [16] M. Izadi and A. K. Sanyal.  Rigid body pose estimation based on the Lagrange–d’Alembert principle.  Automatica, 71:78–88, 2016.
  * [17] M. Kaess, A. Ranganathan, and F. Dellaert.  iSAM: Incremental smoothing and mapping.  IEEE Trans. Robot., 24(6):1365–1378, 2008.
  * [18] N. Kazantzis and C. Kravaris.  Nonlinear observer design using Lyapunov’s auxiliary theorem.  Syst. Control Lett., 34(5):241–247, 1998.
  * [19] G. Kreisselmeier.  Adaptive observers with exponential rate of convergence.  IEEE Trans. Autom. Control, 22(1):2–8, 1977.
  * [20] C. Lageman, J. Trumpf, and R. Mahony.  Gradient-like observers for invariant dynamics on a Lie group.  IEEE Trans. Autom. Control, 55(2):367–377, 2009.
  * [21] P. Lourenço, P. Batista, P. Oliveira, and C. Silvestre.  A globally exponentially stable filter for bearing-only simultaneous localization and mapping with monocular vision.  Robot. Auton. Syst., 100:61–77, 2018.
  * [22] F. Lu and E. Milios.  Globally consistent range scan alignment for environment mapping.  Auton. Robots., 4(4):333–349, 1997.
  * [23] R. Mahony, T. Hamel, and J.-M. Pflimlin.  Nonlinear complementary filters on the special orthogonal group.  IEEE Trans. Autom. Control, 53(5):1203–1218, 2008.
  * [24] I. R. Manchester.  Contracting nonlinear observers: Convex optimization and learning from data.  In Proc. Am. Control Conf., pages 1873–1880. IEEE, 2018.
  * [25] M. Montemerlo and S. Thrun.  FastSLAM: A Scalable Method for the Simultaneous Localization and Mapping Problem in Robotics.  Springer, 2007.
  * [26] R. Ortega, S. Aranovskiy, A. A. Pyrkin, A. Astolfi, and A. A. Bobtsov.  New results on parameter estimation via dynamic regressor extension and mixing: Continuous and discrete-time cases.  IEEE Trans. Autom. Control, 66(5):2265–2272, 2020.
  * [27] R. Ortega, A. Bobtsov, N. Nikolaev, J. Schiffer, and D. Dochain.  Generalized parameter estimation-based observers: Application to power systems and chemical–biological reactors.  Automatica, 129, 2021.  Art. no. 109635.
  * [28] R. Ortega, A. Bobtsov, A. Pyrkin, and S. Aranovskiy.  A parameter estimation approach to state observation of nonlinear systems.  Syst. Control Lett., 85:84–94, 2015.
  * [29] S. Sastry and M. Bodson.  Adaptive Control: Stability, Convergence and Robustness.  Courier Corporation, 2011.
  * [30] H. Strasdat, J. M. Montiel, and A. J. Davison.  Visual SLAM: Why filter?  Image Vis. Comput., 30(2):65–77, 2012.
  * [31] D. Su, H. Kong, S. Sukkarieh, and S. Huang.  Necessary and sufficient conditions for observability of slam-based tdoa sensor array calibration and source localization.  IEEE Trans. Robot., 37(5):1451–1468, 2021.
  * [32] F. Tan, W. Lohmiller, and J.-J. Slotine.  Analytical slam without linearization.  Int. J. Robot. Res., 36(13-14):1554–1578, 2017.
  * [33] P. van Goor, R. Mahony, T. Hamel, and J. Trumpf.  Constructive observer design for visual simultaneous localisation and mapping.  Automatica, 132, 2021.  Art. no. 109803.
  * [34] H. Wang, S. Huang, U. Frese, and G. Dissanayake.  The nonlinearity structure of point feature slam problems with spherical covariance matrices.  Automatica, 49(10):3112–3119, 2013.
  * [35] J. Wang, D. Efimov, and A. A. Bobtsov.  On robust parameter estimation in finite-time without persistence of excitation.  IEEE Trans. Autom. Control, 65(4):1731–1738, 2019.
  * [36] M. Wang, S. Berkane, and A. Tayebi.  Nonlinear observers design for vision-aided inertial navigation systems.  IEEE Trans. Autom. Control, 67(4):1853–1868, 2021.
  * [37] B. Yi, C. Jin, and I. R. Manchester.  Globally convergent visual-feature range estimation with biased inertial measurements.  Automatica, 146, 2022.  Art. no. 110639.
  * [38] B. Yi, C. Jin, L. Wang, G. Shi, and I. R. Manchester.  An almost globally convergent observer for visual SLAM without persistent excitation.  In Proc. IEEE Conf. Decis. Control, pages 5441–5446. IEEE, 2021\.
  * [39] B. Yi, R. Ortega, and W. Zhang.  On state observers for nonlinear systems: A new design and a unifying framework.  IEEE Trans. Autom. Control, 64(3):1193–1200, 2018.
  * [40] B. Yi, L. Wang, and I. R. Manchester.  Attitude estimation from vector measurements: Necessary and sufficient conditions and convergent observer design.  IEEE Trans. Autom. Control, 2023.
  * [41] B. Yi, R. Wang, and I. R. Manchester.  Reduced-order nonlinear observers via contraction analysis and convex optimization.  IEEE Trans. Autom. Control, 67(8):4045–4060, 2021.
  * [42] L. Zhao, S. Huang, and G. Dissanayake.  Linear SLAM: A linear solution to the feature-based and pose graph SLAM based on submap joining.  In IEEE/RSJ Int. Conf. Intell. Robot. Syst., pages 24–30. IEEE, 2013.
  * [43] D. E. Zlotnik and J. R. Forbes.  Gradient-based observer for simultaneous localization and mapping.  IEEE Trans. Autom. Control, 63(12):4338–4344, 2018.