11institutetext:  DIÉNS, ÉNS, CNRS, PSL University, Paris, France
45 rue d’Ulm, 75230, Paris cedex 05, France
11email<EMAIL_ADDRESS>11email<EMAIL_ADDRESS>
# Pattern Recognition Experiments on Mathematical Expressions

David Naccache 11 Ofer Yifrach-Stav 11

###### Abstract

We provide the results of pattern recognition experiments on mathematical
expressions.

We give a few examples of conjectured results. None of which was thoroughly
checked for novelty. We did not attempt to prove all the relations found and
focused on their generation.

##  1 Introduction

Pattern recognition is a process that involves identifying rules in data and
matching them with particular case information. Pattern recognition can be
seen as a type of machine learning, as it uses machine learning algorithms to
recognize patterns in data. This process is characterized by the ability to
learn from data, recognize familiar patterns, and recognize patterns even if
they are partially visible.

Very schematically, there are three main types of pattern recognition
heuristics: statistical pattern recognition, syntactic pattern recognition,
and neural pattern recognition.

  * •

Statistical pattern recognition involves using particular case data to learn
from examples and generalize rules to new observations.

  * •

Syntactic pattern recognition (a.k.a structural pattern recognition), involves
identifying patterns based on simpler sub-patterns called primitives. For
example, opcodes can be seen as primitives that connect to form programs.

  * •

Neural pattern recognition relies on artificial neural networks, which are
made up of many simple processors and their connections. These networks can
learn complex nonlinear input-output relationships and adapt to data through
sequential training procedures.

Most pattern recognition heuristics proceed by two steps:

  * •

An Explorative Stage that seeks to identify patterns

  * •

A Descriptive Stage that categorizes patterns found during exploration

In this work we provide the results of the explorative stage of syntactic
pattern recognition on mathematical expressions. Given the nature of the
objects we work on (conjectures) the descriptive stage is left to a human.

We give a few examples of conjectured results. None of which was thoroughly
checked for novelty. We did not attempt to prove all the relations found and
focused on their generation.

##  2 The Pattern Recognition Algorithm

The pattern recognition algorithm has two components called the generalizer
and the identifier.

The generalizer departs from a known continued fraction or a mathematical
expression (a particular case) and automatically parameterizes parts of it.
The parameterized parts are target ingredients tagged by the user. For each
set of particular parameter values (taken over search space), approximated
values of the formula are collected for later analysis.

Target ingredients are replaced by progressions, denoted by
$\mu_{\mathbf{u}}(i)$, which can be constant, (alternating) arithmetic,
geometric, harmonic or exponential depending on the parameter choices. Those
are captured by the general formula:

$\mu_{\mathbf{u}}(i)=u_{4}i^{u_{5}}+(u_{0}+iu_{1})^{u_{3}}u_{2}^{i}$

For instance, the Ramanujan Machine Project [rama, ref1, ref2] re-discovered
an already known relation involving $e^{\pi}$. Namely, that the continued
fraction defined by $b_{n}=n^{2}+4$ and $a_{n}=2n+1$ converges to:

$\frac{2\left(e^{\pi}+1\right)}{e^{\pi}-1}=1+{\cfrac{1^{2}+4}{3+{\cfrac{2^{2}+4}{5+{\cfrac{3^{2}+4}{7+{\cfrac{4^{2}+4}{9+\ddots\,}}}}}}}}$

A natural tagging query of this identity for search by the user might hence
be:

$Q(\mathbf{u})=\mu_{\mathbf{u}}(0)+{\cfrac{\mu_{\mathbf{v}}(0)}{\mu_{\mathbf{u}}(1)+{\cfrac{\mu_{\mathbf{v}}(1)}{\mu_{\mathbf{u}}(2)+{\cfrac{\mu_{\mathbf{v}}(2)}{\mu_{\mathbf{u}}(3)+{\cfrac{\mu_{\mathbf{v}}(3)}{\mu_{\mathbf{u}}(4)+\ddots\,}}}}}}}}$

With

$\mathbf{u}=\\{\mathbb{Q},\mathbb{Q},1,1,0,0\\}\mbox{~{}~{}and~{}~{}}\mathbf{v}=\\{\mathbb{Z},0,1,1,\mathbb{Q},\mathbb{N}\\}$

That is:

$\mu_{\mathbf{u}}(i)=(\mathbb{Q}+i\mathbb{Q})\mbox{~{}~{}and~{}~{}}\mu_{\mathbf{v}}(i)=\mathbb{Q}i^{\mathbb{N}}+\mathbb{Z}$

When this is done, the program varies the progressions’ parameters over the
chosen search spaces and collects sequences of resulting values. The tests
that we list here are of course non limitative and many other variants can be
added to the proposed heuristic.

###### Remark 1

Obviously, we are quickly limited by the increasing complexity due to nested
loops running over the parameters of the expressions (i.e. the $u_{i}$s).


# Error-Covariance Analysis of Monocular Pose Estimation Using Total Least
Squares

Saeed Maleki111Ph.D. Candidate, Department of Mechanical and Aerospace
Engineering, Email<EMAIL_ADDRESS>University at Buffalo, State
University of New York, Amherst, NY, 14260-4400  John L. Crassidis222SUNY
Distinguished Professor, Moog Endowed Chaired Professor of Innovation,
Department of Mechanical and Aerospace Engineering, Email<EMAIL_ADDRESS>Fellow AIAA. University at Buffalo, State University of New York, Amherst, NY,
14260-4400  Yang Cheng333Associate Professor, Department of Aerospace
Engineering, Email<EMAIL_ADDRESS>Associate Fellow AIAA. Mississippi
State University, Mississippi State, MS, 39762  Matthias Schmid444Research
Assistant Professor, Department of Automotive Engineering, Email:
<EMAIL_ADDRESS>Clemson University, Clemson, SC, 29634

###### Abstract

This study presents a theoretical structure for the monocular pose estimation
problem using the total least squares. The unit-vector line-of-sight
observations of the features are extracted from the monocular camera images.
First, the optimization framework is formulated for the pose estimation
problem with observation vectors extracted from unit vectors from the camera
center-of-projection, pointing towards the image features. The attitude and
position solutions obtained via the derived optimization framework are proven
to reach the Cramér-Rao lower bound under the small angle approximation of the
attitude errors. Specifically, The Fisher Information Matrix and the Cramér-
Rao bounds are evaluated and compared to the analytical derivations of the
error-covariance expressions to rigorously prove the optimality of the
estimates. The sensor data for the measurement model is provided through a
series of vector observations, and two fully populated noise-covariance
matrices are assumed for the body and reference observation data. The inverse
of the former matrices appear in terms of a series of weight matrices in the
cost function. The proposed solution is simulated in a Monte-Carlo framework
with 10,000 samples to validate the error-covariance analysis.

##  1 Introduction

The simultaneous localization and mapping (SLAM) problem focuses on whether a
mobile robot is able to assess situational awareness when put in an unknown
environment and simultaneously make a map of the obstacles, as well as the
possible landmarks on the surrounding objects.

In the SLAM problem, the localization part that determines the orientation
(attitude) of the vehicle can be stated as an attitude estimation problem. The
attitude estimation-only problem can be classified into two main categories
[1]. The first category determines the attitude with respect to a reference
frame using the vector observation data from the surrounding landmark features
in a deterministic manner using multiple simultaneous vector-observations. For
observability purposes, there should be at least two non-collinear features
detected with respect to the body frame of the vehicle.

The second category focuses on filtering the observation data from the
environment by fusing them as a measurement model with a dynamic motion model
of the vehicle trajectories in terms of a state estimation problem [2].
Methods based on the first category can be used to mitigate the transients and
divergence of the filtering approaches, while the second can provide the
estimation even with one observation vector [3]. Every attitude determination
problem involves finding the extremum of a cost function.

One of the early cost functions introduced for the attitude determination is
the Wahba’s problem [4]. Many solutions to solve this problem have been shown
since its introduction [5]. Wahba’s problem is also related to the attitude
and position determination problem [6], which is called pose estimation. This
current work addresses the pose estimation problem in a monocular vision
setup.

Pose estimation is a crucial part of the vision-based navigation applications,
because an accurate pose estimate typically needs to be leveraged for the
vehicle controller to avoid undesirable efforts. Pose estimation solutions
using imagery data-sets can be classified in two main approaches [7]: model-
based and non-model-based approaches.

Model-based solutions use a presumed model of the object using their
dimensions, shape or texture, and corresponds the features to solve for the
model parameters. Template matching [8] and contour tracking [9] are examples
for this class of pose estimation solutions. Non-model-based approaches do not
leverage an object model, but rather determine the pose from a sequence of
images such as structure from motion problem [10]. One solution is information
fusion of high-frequency inertial sensors and a relatively low-frequency
monocular camera for the pose estimation of a rigid body [11]. Reference [12]
provides a comparative study of different methods of initial pose estimation
from monocular camera data for a spacecraft mission application. Attitude
estimation from the two rotated unit-vectors using the geometric algebra is an
interesting rigorous mathematical approach and has been studied in [13]. This
solution can be used for the monocular vision applications as well. In [14], a
review of different pose estimation methods is provided using monocular or
stereo camera image data-sets, in which the available features are either the
coordinates of 3D points in the world or their corresponding 2D projections on
the image plane. Monocular vision can be used for pose estimation of the UAV’s
by only using an off-board camera [15]. ORB-SLAM [mur2015orb] is one of the
real-time solutions for the SLAM problem using monocular camera which is
robust to clutter. Deep learning-based approaches also provide a series of
solutions for relative pose estimation in vision applications [16, 17, 18].
The recovery of feature depth as well as the attitude matrix is given in an
iterative approach in [19]. The pose determination problem is solved in [20]
using point and line correspondences. The first is based on the weak
perspective model of the monocular camera, and the second is a first-order
approximation of the perspective model. The line correspondence solution has
better convergence properties and more accurate estimates compared to the
point correspondence, but comes at the price of higher computational
complexity [11]. Structure from motion is one of the super-sets of pose
estimation problems, in which the 3D coordinates of features as well as the
relative pose of the camera with respect to a reference frame are estimated.
For an in-depth review of the structure from motion algorithms and solutions,
the reader is referred to [10]. Vision-based pose estimation has many
applications in human-computer interaction. Monocular or multiple cameras can
be used to assess the orientation of hand pose [21], in which several degrees
of freedom are solved as unknowns.

While working with visual measurement data, the objects of interest first need
to be segmented. Features are usually then detected, which are finally
corresponded to each other in different image scans. Image segmentation is a
fundamental problem in SLAM using visual data. This has many applications in
robotics [22] such as the autonomous driving, in which the surface,
pedestrians, cars and bikes, etc., need to be detected. Image segmentation can
be stated as classifying the pixels with different labels (semantic
segmentation) or separating the objects of interest as a whole (instance
segmentation). There are a variety of image segmentation methods that range
from simple approaches, such as like kmeans [23], or more computationally
involved and powerful methods, such as deep-learning based methods [24] or
Markov random fields [25]. A survey of deep-learning approaches to solve the
image segmentation problem is shown in [26, 27, 28]. Before initiating the
actually pose determination process, first the feature extraction and feature
correspondence problems must be solved. Features are intuitively the important
points in an image that can be found in other images of the same scene in a
unique way. Feature extraction techniques are categorized into low-level and
high-level approaches. Low-level methods find the local features in an image.
The image instances can be derived from many approaches, such as the Canny
edge detector [29] to SIFT [30] and SURF [31]. High-level features are more
focused on detection of more complex features, such as lines and circles and
other generic shapes. The Hough transform [32] and generalized Hough transform
[33] are some basic solutions using high-level features. A more elaborate
review of feature extraction methods is provided in [34]. Feature
correspondence is also one of the basic problems in image processing and has
many applications in visual SLAM [35]. Grid-based motion statistics is one of
the recent computationally efficient methods for feature matching [36]. A
comparative study of the 2D feature matching methods is provided in [37].

The assumptions of this work are that the segmentation, feature extraction and
feature correspondence steps are solved.

The main focus then will be to solve for the pose estimation problem by
utilizing the pairs of matched features in the format of vector observation
pairs. The pose estimation from vector observation pairs is well-established
in the literature [38]. But the statistical analysis of the estimates, in an
optimal way that achieves the Cramér-Rao lower bound (CRLB) [39], has not been
studied well. The error-covariance expressions in Lidar data sets with the
most generic positive definite matrices for the vector-measurement covariance
are derived in [40] and [41]. The statement of the pose estimation problem as
a total least squares (TLS) problem has been elaborated in [42], in which
there are errors in both the “design matrix” and the measurement observation
[43]. In this paper, the problem of the most general case of sensor
uncertainty in which there are correlations between the different features in
the reference observation vectors, as well as the body observation vectors, is
solved for the unit observation vectors from the camera pointing towards the
features. Furthermore, error-covariance expressions are derived that achieve
the CRLB.

##  2 Overview of Linear Least Squares and Total Least Squares

This section provides a brief introduction to linear and total least squares,
and how they are related and their differences. For a more in-depth review of
the TLS, see [44, 45, 46].

Consider the measurement model of the form

$\tilde{\mathbf{y}}=H\mathbf{x}+\Delta\mathbf{y}$ (1)

where $H$ is a $m\times n$ deterministic matrix with no errors, $\mathbf{x}$
is the $n\times 1$ vector of unknowns, $\tilde{\mathbf{y}}$ is the $m\times 1$
measurement vector, and $\Delta\mathbf{y}$ is the $m\times 1$ measurement
error-vector. The least squares estimate of $\mathbf{x}$ is given by solving
the following problem:

$\begin{gathered}\underset{\mathbf{\hat{x}}}{\min}\text{\
}J=\frac{1}{2}\Delta\mathbf{y}^{T}\Delta\mathbf{y}\\\ \text{subject to:
}\mathbf{\hat{y}}=H\mathbf{\hat{x}}\end{gathered}$ (2)

where the number of measurement samples stacked vertically in the vector
$\tilde{\mathbf{y}}$ should be more than the number of unknowns, and $H$
should have at least rank $n$, for the problem to be observable. The main
underlying assumption in the statistical analysis of least squares is that
$\tilde{\mathbf{y}}$ has a Gaussian distribution with the conditional
likelihood function given by

$p(\mathbf{\tilde{y}}|\mathbf{x})=\frac{1}{(2\pi)^{\frac{m}{2}}\big{[}\det(R_{yy})\big{]}^{\frac{1}{2}}}\text{exp}\left\\{-\frac{1}{2}(\mathbf{\tilde{y}}-H\mathbf{x})^{T}R^{-1}_{yy}(\mathbf{\tilde{y}}-H\mathbf{x})\right\\}$
(3)

where the distribution mean is denoted by $H\mathbf{x}$ and the covariance is
$R_{yy}$. Because of the properties of the exponential function, maximizing
the likelihood function 3 is equivalent to minimizing the negative of the log-
likelihood. The mean and error-covariance of the estimate are given by

$\displaystyle\text{E}\\{\mathbf{\hat{x}}\\}=\mathbf{x}$ (4a)
$\displaystyle\text{cov}\\{\mathbf{\hat{x}}\\}=H^{T}R^{-1}_{yy}H$ (4b)

which shows that the least squares estimate is unbiased.

As stated previously,the design matrix $H$ in the least squares measurement
model in Eq. (1) has no errors. If this underlying assumption does not exist
anymore, which happens in many applications, as will be seen in the SLAM
problem in Section 3, then another formulation must be used to consider the
errors in the design matrix, which leads to the TLS problem, with paramaters
defined by

$\displaystyle\tilde{\mathbf{y}}=\mathbf{y}+\Delta\mathbf{y}$ (5a)
$\displaystyle\mathbf{y}=H\mathbf{x}$ (5b)
$\displaystyle\tilde{H}=H+\Delta{H}$ (5c)

where $\Delta H$ shows the errors in the design matrix. Consider the following
augmented matrix:

$D=\begin{bmatrix}H&\mathbf{y}\end{bmatrix}$ (6)

The conditional likelihood function of the TLS problem is defined by

$p(\tilde{D}|D)=\frac{1}{(2\pi)^{\frac{m}{2}}\big{[}\det(R)\big{]}^{\frac{1}{2}}}\text{exp}\left\\{-\frac{1}{2}\text{vec}(\tilde{D}-D)^{T}R^{-1}\text{vec}(\tilde{D}-D)\right\\}$
(7)

where vec operator stacks all columns of a matrix in a single column. The
maximum likelihood approach for this cost function leads to the minimization
of the log-likelihood function as

$\begin{gathered}J(\hat{D})=\frac{1}{2}\text{vec}(\tilde{D}-\hat{D})^{T}R^{-1}\text{vec}(\tilde{D}-\hat{D})\\\
\text{subject to}:\hat{D}\,{\hat{\mathbf{z}}}=\textbf{0}\\\ \end{gathered}$
(8)

where $\hat{\mathbf{z}}=\begin{bmatrix}\hat{\mathbf{x}}&-1\end{bmatrix}^{T}$.
A unique solution for this problem can be obtained if $\text{rank}(D)=n$.
Also, $R$ is the covariance matrix that accounts for the errors in both
$\tilde{\mathbf{y}}$ and $\tilde{H}$. Although the TLS solution is known to be
biased, the TLS problem is proven to reach the CRLB [47] for the estimate
error-covariance to within first-order error-terms [43]. Closed-form solutions
for the TLS problem are possible only when $R$ is an isotropic matrix.

##  3 Total Least Squares Derivation for Pose Determination

The schematic relating the pose of the reference frame to the body frame is
shown in Figure 1. There are two components that need to be estimated: 1) a
translation vector $\mathbf{p}$ that connects the center of the reference and
body frames, and 2) an attitude matrix $A$ for the relative orientation of the
unit vectors of the coordinate systems. As mentioned in section 1, the sphere
image surface instead of conventional image plane is utilized in this paper.
Therefore, a series of line-of-sight measurements in the form of unit vectors
is used. The terms $u_{i}$ and $v_{i}$ denote the depth of the $i^{th}$
feature on the object of interest. The $\tilde{\mathbf{b}}_{i}$ and
$\tilde{\mathbf{r}}_{i}$ denote the unit vector directions of the line-of-
sight for feature $i$ in reference and body frames, respectively.

Figure 1: Geometric interpretation of the monocular pose estimation problem.

The constraint that relates the unit vectors of projection from both
coordinate systems is given by

$\displaystyle u_{i}\mathbf{b}_{i}=v_{i}A\mathbf{r}_{i}-\mathbf{p}\text{,\ \ \
}i=1,...,n$ (9)

In which $n$ shows the number of features in every scan of the monocular
camera. In this study, the camera is assumed to be intrinsically calibrated
and only the extrinsic parameters of attitude and position are sought.
Equation (9) is a measurement model for the estimation problem. In this paper,
the $\tilde{.}$ shows the measurement signal and the $\hat{.}$ refers to the
estimated value of the unknowns. The Eq. (9) can be written as

$\begin{split}u_{i}\mathbf{b}_{i}&=v_{i}A\mathbf{r}_{i}-\mathbf{p}\\\
&=v_{i}\begin{bmatrix}\mathbf{r}^{T}_{i}&\mathbf{0}_{1\times
3}&\mathbf{0}_{1\times 3}\\\ \mathbf{0}_{1\times
3}&\mathbf{r}^{T}_{i}&\mathbf{0}_{1\times 3}\\\ \mathbf{0}_{1\times
3}&\mathbf{0}_{1\times
3}&\mathbf{r}^{T}_{i}\end{bmatrix}\begin{bmatrix}\mathbf{a}_{1}\\\
\mathbf{a}_{2}\\\ \mathbf{a}_{3}\end{bmatrix}-\mathbf{p}\\\
&=u_{i}H_{i}\mathbf{x}-\mathbf{p}\\\ &\equiv\mathbf{y}_{i}\end{split}$ (10)

where $H_{i}$ is the design matrix for the $i^{th}$ feature and
$\mathbf{a}_{i}$, $i=1,\,2,\,3$, are the columns of the attitude matrix $A$.
The perfect measurement model however is not realistic because of noise in the
design matrix as well as the observation vectors of Eq.
(LABEL:eqn:det_tls_sensor_model). Therefore, in the actual version of the
sensor model in Eq. (LABEL:eqn:det_tls_sensor_model), the following relation
is used:

$\begin{split}\left(\tilde{u}_{i}-\Delta
u_{i}\right)\left({\tilde{\mathbf{b}}_{i}}-\boldsymbol{\Delta}\mathbf{b}_{i}\right)&=\left(\tilde{v}_{i}-\Delta
v_{i}\right)A({\tilde{\mathbf{r}}_{i}}-\boldsymbol{\Delta}\mathbf{r}_{i})-\mathbf{p}\\\
&=(\tilde{H}_{i}-\Delta H_{i}){\mathbf{x}}-\mathbf{p}\\\
&\equiv{\tilde{\mathbf{y}}_{i}}-\boldsymbol{\Delta}\mathbf{y}_{i}\end{split}$
(11)

where the design matrix $\tilde{H}_{i}$ and the observation vector
$\tilde{\mathbf{y}}_{i}$ have the errors of $\Delta H_{i}$ and
$\Delta\mathbf{y}_{i}$, respectively. Because the model is linear in terms of
the unknowns $\mathbf{x}$ and the translation vector $\mathbf{p}$, then the
problem can be posed using a TLS formulation with the constraint

$\hat{u}_{i}{\hat{\mathbf{b}}_{i}}=\hat{v}_{i}\hat{A}{\hat{\mathbf{r}}_{i}}-{\hat{\mathbf{p}}}$
(12)

which shows the structure of a TLS problem because of the erroneous design
matrix. It is already well-known that an image from a monocular camera does
not provide us with depth information of the features. To avoid observability
problems, suppose that rough measurements with large covariance of all the
scale factors in the both frames are provided. Therefore, assume that $u_{i}$
and $v_{i}$ are measured and denoted by $\tilde{u}_{i}$ and $\tilde{v}_{i}$
with the covariances of $R_{u_{i}}$ and $R_{v_{i}}$. The cost function is

$\begin{split}J(\tilde{u}_{i},\hat{u}_{i},\tilde{v}_{i},\hat{v}_{i},\tilde{\mathbf{b}}_{i},\hat{\mathbf{b}}_{i},\tilde{\mathbf{r}}_{i},\hat{\mathbf{r}}_{i})&=\frac{1}{2}\sum_{i=1}^{n}(\tilde{u}_{i}-\hat{u}_{i})^{T}R^{-1}_{u_{i}}(\tilde{u}_{i}-\hat{u}_{i})+\frac{1}{2}\sum_{i=1}^{n}(\tilde{v}_{i}-\hat{v}_{i})^{T}R^{-1}_{v_{i}}(\tilde{v}_{i}-\hat{v}_{i})\\\
&+\frac{1}{2}\sum_{i=1}^{n}(\tilde{\mathbf{b}}_{i}-\mathbf{\hat{b}}_{i})^{T}R_{b_{i}}^{-1}(\tilde{\mathbf{b}}_{i}-\mathbf{\hat{b}}_{i})+(\tilde{\mathbf{r}}_{i}-\mathbf{\hat{r}}_{i})^{T}R_{r_{i}}^{-1}(\tilde{\mathbf{r}}_{i}-\mathbf{\hat{r}}_{i})\end{split}$
(13)

subject to the constraints

$\displaystyle\hat{u}_{i}\mathbf{\hat{b}}_{i}=\hat{v}_{i}\hat{A}\mathbf{\hat{r}}_{i}-\mathbf{\hat{p}}\text{\
\ \ }i=1,...,n$ (14a) $\displaystyle\hat{A}\hat{A}^{T}=I_{3\times 3},\text{\
}\text{det}(\hat{A})=1$ (14b)

Define the vector $\mathbf{d}_{i}$ as follows:

$\mathbf{d}_{i}=\begin{bmatrix}\mathbf{r}_{i}\\\ \mathbf{b}_{i}\end{bmatrix}$
(15)

The cost function needs to augmented to include the constraint in Eq. (14a),
which is accomplished using Lagrange multiples, denoted by
$\boldsymbol{\lambda}_{i}$:

$\begin{split}J_{a}&=\frac{1}{2}\sum_{i=1}^{n}(\tilde{u}_{i}-\hat{u}_{i})^{T}R^{-1}_{u_{i}}(\tilde{u}_{i}-\hat{u}_{i})+\frac{1}{2}\sum_{i=1}^{n}(\tilde{v}_{i}-\hat{v}_{i})^{T}R^{-1}_{v_{i}}(\tilde{v}_{i}-\hat{v}_{i})\\\
&+\frac{1}{2}\sum_{i=1}^{n}(\tilde{\mathbf{d}}_{i}-\mathbf{\hat{d}}_{i})^{T}R_{i}^{-1}(\tilde{\mathbf{d}}_{i}-\mathbf{\hat{d}}_{i})+2\boldsymbol{\lambda}_{i}^{T}(-\hat{u}_{i}\mathbf{\hat{b}}_{i}+\hat{v}_{i}\hat{A}\mathbf{\hat{r}}_{i}-\mathbf{\hat{p}})\end{split}$
(16)

The necessary condition of this augmented cost function for
$\mathbf{\hat{d}}_{i}$ will be

$\frac{\partial
J_{a}}{\partial\mathbf{\hat{d}}_{i}}=-R^{-1}_{i}(\mathbf{\tilde{d}}_{i}-\mathbf{\hat{d}}_{i})+\hat{S}^{T}_{i}\boldsymbol{\lambda}_{i}=\mathbf{0}$
(17)

in which

$\displaystyle
R_{i}\equiv\text{E}\\{\boldsymbol{\Delta}\mathbf{d}_{i}\boldsymbol{\Delta}\mathbf{d}^{T}_{i}\\}=\begin{bmatrix}R_{r_{i}}&0_{3\times
3}\\\ 0_{3\times 3}&R_{b_{i}}\end{bmatrix}$ (18a)
$\displaystyle\hat{S}_{i}=\begin{bmatrix}\hat{v}_{i}\hat{A}&-\hat{u}_{i}I_{3\times
3}\end{bmatrix}$ (18b) $\displaystyle
R_{r_{i}}=\text{E}\\{\boldsymbol{\Delta}\mathbf{r}_{i}\boldsymbol{\Delta}\mathbf{r}^{T}_{i}\\}$
(18c) $\displaystyle
R_{b_{i}}=\text{E}\\{\boldsymbol{\Delta}\mathbf{b}_{i}\boldsymbol{\Delta}\mathbf{b}^{T}_{i}\\}$
(18d) $\displaystyle R_{u_{i}}=\text{E}\\{\Delta u^{2}_{i}\\}$ (18e)
$\displaystyle R_{v_{i}}=\text{E}\\{\Delta v^{2}_{i}\\}$ (18f)

From the previous necessary condition in Eq. (17), for the features in an
image scan $i=1,...,n$, the following is given:

$\mathbf{\hat{d}}_{i}=\mathbf{\tilde{d}}_{i}-R_{i}\hat{S}^{T}_{i}\boldsymbol{\lambda}_{i}$
(19)

Then the Lagrangian multiplier $\boldsymbol{\lambda}_{i}$ can be computed as

$\boldsymbol{\lambda}_{i}=Q^{-1}_{\hat{\lambda}_{i}}(\hat{S}_{i}\mathbf{\tilde{d}}_{i}-\mathbf{\hat{p}})$
(20)

in which

$Q_{\hat{\lambda}_{i}}=\hat{S}_{i}R_{i}\hat{S}^{T}_{i}$ (21)

The cost function in Eq. (16) can be re-written as

$\begin{split}J_{a}&=\frac{1}{2}\sum_{i=1}^{n}(\tilde{u}_{i}-\hat{u}_{i})^{T}R^{-1}_{u_{i}}(\tilde{u}_{i}-\hat{u}_{i})+\frac{1}{2}\sum_{i=1}^{n}(\tilde{v}_{i}-\hat{v}_{i})^{T}R^{-1}_{v_{i}}(\tilde{v}_{i}-\hat{v}_{i})\\\
&+\frac{1}{2}\sum_{i=1}^{n}(\tilde{\mathbf{d}}_{i}-\mathbf{\hat{d}}_{i})^{T}R^{-1}_{i}(\tilde{\mathbf{d}}_{i}-\mathbf{\hat{d}}_{i})+2\boldsymbol{\lambda}^{T}_{i}(\hat{S}_{i}\mathbf{\hat{d}}_{i}-\mathbf{\hat{p}})\end{split}$
(22)

From Eq. (19) and Eq. (20)

$\mathbf{\tilde{d}}_{i}-\mathbf{\hat{d}}_{i}=R_{i}\hat{S}^{T}_{i}Q^{-1}_{\hat{\lambda}_{i}}(-\hat{u}_{i}\mathbf{\tilde{b}}_{i}+\hat{v}_{i}\hat{A}\mathbf{\tilde{r}}_{i}-\mathbf{\hat{p}})$
(23)

Then the cost function in Eq. (13) can be rewritten as

$\begin{split}J&=\frac{1}{2}\sum_{i=1}^{n}(\tilde{u}_{i}-\hat{u}_{i})^{T}R^{-1}_{u_{i}}(\tilde{u}_{i}-\hat{u}_{i})+\frac{1}{2}\sum_{i=1}^{n}(\tilde{v}_{i}-\hat{v}_{i})^{T}R^{-1}_{v_{i}}(\tilde{v}_{i}-\hat{v}_{i})\\\
&+\frac{1}{2}\sum_{i=1}^{n}\left(\hat{u}_{i}\mathbf{\tilde{b}}_{i}-\hat{v}_{i}\hat{A}\mathbf{\tilde{r}}_{i}+\mathbf{\hat{p}}\right)^{T}Q^{-1}_{\hat{\lambda}_{i}}\left(\hat{u}_{i}\mathbf{\tilde{b}}_{i}-\hat{v}_{i}\hat{A}\mathbf{\tilde{r}}_{i}+\mathbf{\hat{p}}\right)\end{split}$
(24)

It is to be noted that the TLS solution has an extra capability of providing
estimates for the vector observations, which are the positions of the vehicle
with respect to the landmark features in the environment, as well the unknown
pose. This can be compared to the other solutions for the pose estimation
problem in a sense that makes the TLS a proper solution for the more generic
SLAM problem. Also the TLS solution provides the measurement residuals based
on Eq. (23), which are the difference of the measurements with the estimates.
This is very useful to provide a measure of the accuracy of the algorithms, as
well as a good reference for the tuning of the measurement covariances for the
real applications.

###  3.1 Linear Attitude Measurement model

The relation between the true and estimated attitude matrix can be expressed
as

$\hat{A}=\text{exp}(-[\boldsymbol{\delta}\boldsymbol{\alpha}\times])A$ (25)

where $[.\times]$ denotes the cross product matrix of a vector [1]. Using a
small angle assumption which is a first-order approximation of the attitude
error, the attitude estimate can be written as

$\hat{A}\approx\big{(}I_{3\times
3}-[\boldsymbol{\delta}\boldsymbol{\alpha}\times]\big{)}A$ (26)

Note that for a first-order approximation of the covariance, a second-order
approximation of the cost function is required. The error terms inside of the
summations in Eq. (24) is approximated as

$\begin{split}\hat{u}_{i}\mathbf{\tilde{b}}_{i}-\hat{v}_{i}\hat{A}\mathbf{\tilde{r}}_{i}+\mathbf{\hat{p}}&=(u_{i}+\delta
u_{i})(\mathbf{b}_{i}+\boldsymbol{\Delta}\mathbf{b}_{i})-(v_{i}+\delta
v_{i})\big{(}I_{3\times
3}-[\boldsymbol{\delta}\boldsymbol{\alpha}\times]\big{)}A(\mathbf{r}_{i}+\boldsymbol{\Delta}\mathbf{r}_{i})+(\mathbf{p}+\boldsymbol{\delta}\mathbf{p})\\\
&\approx(u_{i}\mathbf{b}_{i}-v_{i}A\mathbf{r}_{i}+\mathbf{p})-G_{i}\boldsymbol{\delta}\mathbf{x}+\boldsymbol{\Delta}\mathbf{a}_{i}\end{split}$
(27)

where the first term is zero based on the constraint in Eq. (9) and

$\displaystyle G_{i}=\begin{bmatrix}v_{i}[A\mathbf{r}_{i}\times]&-I_{3\times
3}&0_{3\times 2(i-1)}&-\mathbf{b}_{i}&A\mathbf{r}_{i}&0_{3\times
2(n-i)}\end{bmatrix}_{3\times(6+2n)}$ (28a)
$\displaystyle\boldsymbol{\Delta}\mathbf{a}_{i}=u_{i}\boldsymbol{\Delta}\mathbf{b}_{i}-v_{i}A\boldsymbol{\Delta}\mathbf{r}_{i}$
(28b)
$\displaystyle\boldsymbol{\delta}\mathbf{x}=\begin{bmatrix}\boldsymbol{\delta}\boldsymbol{\alpha}^{T}&\boldsymbol{\delta}\mathbf{p}^{T}&\delta
u_{1}&\delta v_{1}&...&\delta u_{i}&\delta v_{i}&...&\delta u_{n}&\delta
v_{n}\end{bmatrix}^{T}_{(6+2n)\times 1}$ (28c)

It is to be noted that the term $Q_{\hat{\lambda}_{i}}$ in Eq. (24) is also a
function of the unknowns and has an approximation within first order of
estimate errors $\boldsymbol{\delta}\mathbf{x}$ which is derived from

$\begin{split}Q_{\hat{\lambda}_{i}}=&\hat{v}^{2}_{i}\hat{A}R_{r_{i}}\hat{A}^{T}+\hat{u}^{2}_{i}R_{b_{i}}\\\
=&(v_{i}+\delta v_{i})^{2}\big{(}I_{3\times
3}-[\boldsymbol{\delta}\boldsymbol{\alpha}\times]\big{)}AR_{r_{i}}A^{T}\big{(}I_{3\times
3}-[\boldsymbol{\delta}\boldsymbol{\alpha}\times]\big{)}^{T}+(u_{i}+\delta
u_{i})^{2}R_{b_{i}}\end{split}$ (29)

Decomposing $Q_{\hat{\lambda}_{i}}$ to first-order and second-order terms
gives

$\displaystyle Q_{\hat{\lambda}_{i}}=Q_{\lambda_{i}}+\delta
Q_{\lambda_{i}}+\delta^{2}Q_{\lambda_{i}}$ (30a) $\displaystyle
Q_{\lambda_{i}}=v^{2}_{i}AR_{r_{i}}A^{T}+u^{2}_{i}R_{b_{i}}=S_{i}R_{i}S^{T}_{i}$
(30b) $\displaystyle\delta
Q_{\lambda_{i}}(\boldsymbol{\delta}\boldsymbol{\alpha})=2v_{i}\delta
v_{i}AR_{r_{i}}A^{T}-v^{2}_{i}[\boldsymbol{\delta}\boldsymbol{\alpha}\times]AR_{r_{i}}A^{T}-v^{2}_{i}AR_{r_{i}}A^{T}[\boldsymbol{\delta}\boldsymbol{\alpha}\times]+2u_{i}\delta
u_{i}R_{b_{i}}$ (30c) $\displaystyle\delta^{2}Q_{\lambda_{i}}=\delta
v^{2}_{i}AR_{r_{i}}A^{T}+v^{2}_{i}[\boldsymbol{\delta}\boldsymbol{\alpha}\times]AR_{r_{i}}A^{T}[\boldsymbol{\delta}\boldsymbol{\alpha}\times]^{T}-2v_{i}\delta
v_{i}\left([\boldsymbol{\delta}\boldsymbol{\alpha}\times]AR_{r_{i}}A^{T}+AR_{r_{i}}A^{T}[\boldsymbol{\delta}\boldsymbol{\alpha}\times]^{T}\right)+\delta
u^{2}_{i}R_{b_{i}}$ (30d)

The inverse of $Q_{\hat{\lambda}_{i}}$ is given by

$Q^{-1}_{\hat{\lambda_{i}}}=Q^{-1}_{\lambda_{i}}-Q^{-1}_{\lambda_{i}}\delta
Q_{\lambda_{i}}Q^{-1}_{\lambda_{i}}-Q^{-1}_{\lambda_{i}}\delta^{2}Q_{\lambda_{i}}Q^{-1}_{\lambda_{i}}$
(31)

So the constant term for $Q^{-1}_{\hat{\lambda_{i}}}$ will be
$Q^{-1}_{\lambda_{i}}$ for which the inverse is defined in Eq. (30a). The
first-order terms of estimate errors are already given in the quadratic terms
in Eq. (24). Then only a part of the $Q_{\hat{\lambda}_{i}}$, denoted by
$Q_{\lambda_{i}}$, which is not a function of unknown errors
$\boldsymbol{\delta}\mathbf{x}$, contributes to the second-order approximation
of the cost function. The second-order cost function from the first-order
estimate errors gives

$\begin{split}L=\frac{1}{2}\sum_{i=1}^{n}(\Delta
u_{i}-\mathbf{e}_{i}\boldsymbol{\delta}\mathbf{x})^{T}R^{-1}_{u_{i}}(\Delta
u_{i}-\mathbf{e}_{i}\boldsymbol{\delta}\mathbf{x})+(\Delta
v_{i}-\mathbf{f}_{i}\boldsymbol{\delta}\mathbf{x})^{T}R^{-1}_{v_{i}}(\Delta
v_{i}-\mathbf{f}_{i}\boldsymbol{\delta}\mathbf{x})+\frac{1}{2}\sum_{i=1}^{n}(\boldsymbol{\Delta}\mathbf{a}_{i}-G_{i}\boldsymbol{\delta}\mathbf{x})^{T}Q^{-1}_{\lambda_{i}}(\boldsymbol{\Delta}\mathbf{a}_{i}-G_{i}\boldsymbol{\delta}\mathbf{x})\end{split}$
(32)

in which

$\displaystyle\mathbf{e}_{i}=\begin{bmatrix}0_{1\times(4+2i)}&1&0_{1\times(2n-2i+1)}\end{bmatrix}_{1\times(6+2n)}$
(33a)
$\displaystyle\mathbf{f}_{i}=\begin{bmatrix}0_{1\times(5+2i)}&1&0_{1\times(2n-2i)}\end{bmatrix}_{1\times(6+2n)}$
(33b)

Note that Eq. (32) is analogous to second-order approximation of the log-
likelihood function in Eq. (8).

###  3.2 Covariance Analysis of the Estimates and Residuals

In this section, the analytical expressions for the covariance of estimates
and measurement residuals are derived. The necessary conditions for the
unknowns from the second-order cost function in Eq. (32) yields

$\begin{split}\frac{\partial
L}{\partial\boldsymbol{\delta}\mathbf{x}}&=-\sum_{i=1}^{n}\mathbf{e}^{T}_{i}R^{-1}_{u_{i}}\left(\Delta
u_{i}-\mathbf{e}_{i}\boldsymbol{\delta}\mathbf{x}\right)-\sum_{i=1}^{n}\mathbf{f}^{T}_{i}R^{-1}_{v_{i}}\left(\Delta
v_{i}-\mathbf{f}_{i}\boldsymbol{\delta}\mathbf{x}\right)-\sum_{i=1}^{n}G^{T}_{i}Q^{-1}_{\lambda_{i}}\left(\boldsymbol{\Delta}\mathbf{a}_{i}-G_{i}\boldsymbol{\delta}\mathbf{x}\right)\\\
&=F\boldsymbol{\delta}\mathbf{x}-\mathbf{g}\\\ &=\mathbf{0}_{(6+2n)\times
1}\end{split}$ (34)

Where

$\displaystyle
F=\sum_{i=1}^{n}\mathbf{e}^{T}_{i}R^{-1}_{u_{i}}\mathbf{e}_{i}+\mathbf{f}^{T}_{i}R^{-1}_{v_{i}}\mathbf{f}_{i}+G^{T}_{i}Q^{-1}_{\lambda_{i}}G_{i}$
(35a)
$\displaystyle\mathbf{g}=\sum_{i=1}^{n}\mathbf{e}^{T}_{i}R^{-1}_{u_{i}}\Delta
u_{i}+\mathbf{f}^{T}_{i}R^{-1}_{v_{i}}\Delta
v_{i}+G^{T}_{i}Q^{-1}_{\lambda_{i}}\boldsymbol{\Delta}\mathbf{a}_{i}$ (35b)

Then the optimal vector of unknowns will be

$\boldsymbol{\delta}\mathbf{x}=F^{-1}\mathbf{g}$ (36)

####  3.2.1 Estimate Covatiance

The estimate covariance of $\boldsymbol{\delta}\mathbf{x}$ is given

$\begin{split}\text{cov}\\{\boldsymbol{\delta}\mathbf{x}\\}&\equiv\text{E}\\{\boldsymbol{\delta}\mathbf{x}\boldsymbol{\delta}\mathbf{x}^{T}\\}\\\
&=\text{E}\\{F^{-1}\mathbf{g}\mathbf{g}^{T}F^{-T}\\}\\\
&=F^{-1}\text{E}\\{\mathbf{g}\mathbf{g}^{T}\\}F^{-T}\end{split}$ (37)

For calculation of $\text{E}\\{\mathbf{g}\mathbf{g}^{T}\\}$ series of
identities is required, listed here:

$\displaystyle\text{E}\\{\boldsymbol{\Delta}\mathbf{a}_{i}\boldsymbol{\Delta}\mathbf{a}^{T}_{i}\\}=Q_{\lambda_{i}}$
(38a) $\displaystyle\text{E}\\{\Delta
v_{i}\boldsymbol{\Delta}\mathbf{a}^{T}_{i}\\}=\mathbf{0}_{1\times 3}$ (38b)
$\displaystyle\text{E}\\{\Delta
u_{i}\boldsymbol{\Delta}\mathbf{a}^{T}_{i}\\}=\mathbf{0}_{1\times 3}$ (38c)

Proof for Eq. (38a): Based on the definition of
$\boldsymbol{\Delta}\mathbf{a}_{i}$ in Eq. (28b)

$\begin{split}P_{\boldsymbol{\Delta}\mathbf{a}_{i}}&\equiv\text{E}\\{\boldsymbol{\Delta}\mathbf{a}_{i}\boldsymbol{\Delta}\mathbf{a}^{T}_{i}\\}\\\
&=\text{E}\\{\left(u_{i}\boldsymbol{\Delta}\mathbf{b}_{i}-v_{i}A\boldsymbol{\Delta}\mathbf{r}_{i}\right)\left(u_{i}\boldsymbol{\Delta}\mathbf{b}_{i}-v_{i}A\boldsymbol{\Delta}\mathbf{r}_{i}\right)^{T}\\}\\\
&=u^{2}_{i}R_{b_{i}}+v^{2}_{i}AR_{r_{i}}A^{T}\\\ &=Q_{\lambda_{i}}\end{split}$
(39)

in which the definition of $Q_{\lambda_{i}}$ in Eq. (30b) and the fact that
the cross-covariance $R_{rb_{i}}$ is zero are used. Eq. (38b) is also based on
the assumption that the cross covariance of the measured depth and the
observation direction measurements $\tilde{\mathbf{r}}_{i}$ and
$\tilde{\mathbf{b}}_{i}$ are zero which is denoted in Eq. (38b) and Eq. (38c).
Using the derivation of estimate covariance for
$\boldsymbol{\delta}\mathbf{x}$ from Eq. (37) gives

$\begin{split}\text{E}\\{\mathbf{g}\mathbf{g}^{T}\\}&=\text{E}\Big{\\{}\left(\sum_{i=1}^{n}\mathbf{e}^{T}_{i}R^{-1}_{u_{i}}\Delta
u_{i}+\mathbf{f}^{T}_{i}R^{-1}_{v_{i}}\Delta
v_{i}+G^{T}_{i}Q^{-1}_{\lambda_{i}}\boldsymbol{\Delta}\mathbf{a}_{i}\right)\left(\sum_{i=1}^{n}\mathbf{e}^{T}_{i}R^{-1}_{u_{i}}\Delta
u_{i}+\mathbf{f}^{T}_{i}R^{-1}_{v_{i}}\Delta
v_{i}+G^{T}_{i}Q^{-1}_{\lambda_{i}}\boldsymbol{\Delta}\mathbf{a}_{i}\right)^{T}\Big{\\}}\\\
&=\sum_{i=1}^{n}\left(\mathbf{e}^{T}_{i}R^{-1}_{u_{i}}\text{E}\\{\Delta
u^{2}_{i}\\}R^{-1}_{u_{i}}\mathbf{e}_{i}+\mathbf{f}^{T}_{i}R^{-1}_{v_{i}}\text{E}\\{\Delta
v^{2}_{i}\\}R^{-1}_{v_{i}}\mathbf{f}_{i}\right)\\\
&+\sum_{i=1}^{n}G^{T}_{i}Q^{-1}_{\lambda_{i}}\text{E}\\{\boldsymbol{\Delta}\mathbf{a}_{i}\Delta\mathbf{a}^{T}_{i}\\}Q^{-1}_{\lambda_{i}}G_{i}\\\
&+\sum_{j=1}^{n}\mathbf{e}^{T}_{j}R^{-1}_{u_{j}}\sum_{i=1}^{n}\text{E}\\{\Delta
u_{j}\Delta\mathbf{a}^{T}_{i}Q^{-T}_{\lambda_{i}}G_{i}\\}+\sum_{j=1}^{n}\mathbf{f}^{T}_{j}R^{-1}_{v_{j}}\sum_{i=1}^{n}\text{E}\\{\Delta
v_{j}\Delta\mathbf{a}^{T}_{i}Q^{-T}_{\lambda_{i}}G_{i}\\}\\\
&+\Big{[}\sum_{j=1}^{n}\mathbf{e}^{T}_{j}R^{-1}_{u_{j}}\sum_{i=1}^{n}\text{E}\\{\Delta
u_{j}\Delta\mathbf{a}^{T}_{i}Q^{-T}_{\lambda_{i}}G_{i}\\}+\sum_{j=1}^{n}\mathbf{f}^{T}_{j}R^{-1}_{v_{j}}\sum_{i=1}^{n}\text{E}\\{\Delta
v_{j}\Delta\mathbf{a}^{T}_{i}Q^{-T}_{\lambda_{i}}G_{i}\\}\Big{]}^{T}\\\
&=\sum_{i=1}^{n}\left(\mathbf{e}^{T}_{i}R^{-1}_{u_{i}}R_{u_{i}}R^{-1}_{u_{i}}\mathbf{e}_{i}+\mathbf{f}^{T}_{i}R^{-1}_{v_{i}}R_{v_{i}}R^{-1}_{v_{i}}\mathbf{f}_{i}+G^{T}_{i}Q^{-1}_{\lambda_{i}}Q_{\lambda_{i}}Q^{-1}_{\lambda_{i}}G_{i}\right)\\\
&=\sum_{i=1}^{n}\left(\mathbf{e}^{T}_{i}R^{-1}_{u_{i}}\mathbf{e}_{i}+\mathbf{f}^{T}_{i}R^{-1}_{v_{i}}\mathbf{f}_{i}+G^{T}_{i}Q^{-1}_{\lambda_{i}}G_{i}\right)\end{split}$
(40)

in which based on Eq. (38b), the last two of the 4 terms are zero. The fact
that the correlation between the different features are zero, i.e.
$\text{E}\\{\boldsymbol{\Delta}\mathbf{a}_{i}\Delta\mathbf{a}^{T}_{j}\\}=0$
for $i\neq j$, is also used. The remaining terms add up to match the
definition of the FIM in Eq. (35a) and therefore from Eq. (37)

$\begin{split}\text{cov}\\{\boldsymbol{\delta}\mathbf{x}\\}&=F^{-1}\text{E}\\{\mathbf{g}\mathbf{g}^{T}\\}F^{-T}\\\
&=F^{-1}FF^{-1}\\\ &=F^{-1}\end{split}$ (41)

####  3.2.2 Residual Covariance for Measurements

As mentioned earlier in section , the TLS solution estimates the vector
observations. This makes the TLS a potential solution for the SLAM problem in
which the location of the vehicle with respect to the environment landmarks or
features is needed, in addition to solving for the vehicle’s pose. For the
residual covariances of the observation vectors, Eq. (23) gives

$\begin{split}\mathbf{\tilde{d}}_{i}-\mathbf{\hat{d}}_{i}&\equiv\mathbf{d}_{i}+\boldsymbol{\Delta}\mathbf{d}_{i}-(\mathbf{d}_{i}+\boldsymbol{\delta}\mathbf{d}_{i})\\\
&=\boldsymbol{\Delta}\mathbf{d}_{i}-\boldsymbol{\delta}\mathbf{d}_{i}\\\
&=-R_{i}\hat{S}^{T}_{i}Q^{-1}_{\hat{\lambda}_{i}}\left(\hat{u}_{i}\mathbf{\tilde{b}}_{i}-\hat{v}_{i}\hat{A}\mathbf{\tilde{r}}_{i}+\mathbf{\hat{p}}\right)\end{split}$
(42)

For the first-order approximation, Eq. (27) yields

$\boldsymbol{\Delta}\mathbf{d}_{i}-\boldsymbol{\delta}\mathbf{d}_{i}\approx-
C_{i}(\boldsymbol{\Delta}\mathbf{a}_{i}-G_{i}\boldsymbol{\delta}\mathbf{x})$
(43)

where $C_{i}=R_{i}S^{T}_{i}Q^{-1}_{\lambda_{i}}$. Note that the terms
$\hat{S}_{i}$ and $Q_{\hat{\lambda}_{i}}$ in Eq. (42) are also function of the
unknown attitude and position, but their first-order approximations cannot
affect the final answer for the first-order error because the first-order
error terms inside of the parenthesis in Eq. (42) are already given. Then the
covariance of the measurement residual within the first order of errors will
be

$\begin{split}\text{cov}\\{\boldsymbol{\Delta}\mathbf{d}_{i}-\boldsymbol{\delta}\mathbf{d}_{i}\\}&\equiv\text{E}\\{(\boldsymbol{\Delta}\mathbf{d}_{i}-\boldsymbol{\delta}\mathbf{d}_{i})(\boldsymbol{\Delta}\mathbf{d}_{i}-\boldsymbol{\delta}\mathbf{d}_{i})^{T}\\}\\\
&\approx
C_{i}\text{E}\Big{\\{}\boldsymbol{\Delta}\mathbf{a}_{i}-G_{i}\boldsymbol{\delta}\mathbf{x})(\boldsymbol{\Delta}\mathbf{a}_{i}-G_{i}\boldsymbol{\delta}\mathbf{x})^{T}\big{\\}}C^{T}_{i}\\\
&=C_{i}\Big{[}\text{E}\\{\boldsymbol{\Delta}\mathbf{a}_{i}\boldsymbol{\Delta}\mathbf{a}^{T}_{i}\\}-\text{E}\\{G_{i}\boldsymbol{\delta}\mathbf{x}\boldsymbol{\Delta}\mathbf{a}^{T}_{i}\\}-\text{E}\\{G_{i}\boldsymbol{\delta}\mathbf{x}\boldsymbol{\Delta}\mathbf{a}^{T}_{i}\\}^{T}+G_{i}\text{cov}\\{\boldsymbol{\delta}\mathbf{x}\\}G^{T}_{i}\Big{]}C^{T}_{i}\end{split}$
(44)

From Eq. (39) and based on the identity of

$\displaystyle\text{E}\\{G_{i}\boldsymbol{\delta}\mathbf{x}\boldsymbol{\Delta}\mathbf{a}^{T}_{i}\\}=G_{i}F^{-1}G^{T}_{i}$
(45)

the residual covariance will be

$\begin{split}\text{cov}\\{\boldsymbol{\Delta}\mathbf{d}_{i}-\boldsymbol{\delta}\mathbf{d}_{i}\\}&=C_{i}(Q_{\lambda_{i}}-G_{i}F^{-1}G^{T}_{i}-G_{i}F^{-1}G^{T}_{i}+G_{i}F^{-1}G^{T}_{i})C^{T}_{i}\\\
&=C_{i}(Q_{\lambda_{i}}-G_{i}F^{-1}G^{T}_{i})C^{T}_{i}\end{split}$ (46)

Proof for Eq. (45) : From the optimal value of $\boldsymbol{\delta}\mathbf{x}$
in Eq. (36), the following expression is given:

$\begin{split}\text{E}\\{G_{i}\boldsymbol{\delta}\mathbf{x}\boldsymbol{\Delta}\mathbf{a}^{T}_{i}\\}&=\text{E}\\{G_{i}F^{-1}\mathbf{g}\boldsymbol{\Delta}\mathbf{a}^{T}_{i}\\}\\\
&=\text{E}\Big{\\{}G_{i}F^{-1}\Big{[}\sum_{j=1}^{n}\mathbf{e}^{T}_{j}R^{-1}_{u_{j}}\Delta
u_{j}\boldsymbol{\Delta}\mathbf{a}^{T}_{i}+\mathbf{f}^{T}_{j}R^{-1}_{v_{j}}\Delta
v_{j}\boldsymbol{\Delta}\mathbf{a}^{T}_{i}+G^{T}_{j}Q^{-1}_{\lambda_{j}}\boldsymbol{\Delta}\mathbf{a}_{j}\boldsymbol{\Delta}\mathbf{a}^{T}_{i}\Big{]}\Big{\\}}\end{split}$
(47)

From the assumption that the correlation between the depth measurement errors
$\Delta u_{i}$ and $\Delta v_{i}$ and the direction measurement residuals
$\boldsymbol{\Delta}\mathbf{a}_{i}$ are zero, also from Eq. (39), the
following expression is given:

$\begin{split}\text{E}\\{G_{i}\boldsymbol{\delta}\mathbf{x}\boldsymbol{\Delta}\mathbf{a}^{T}_{i}\\}&=\text{E}\Big{\\{}G_{i}F^{-1}\Big{[}\sum_{j=1}^{n}G^{T}_{j}Q^{-1}_{\lambda_{j}}\boldsymbol{\Delta}\mathbf{a}_{j}\boldsymbol{\Delta}\mathbf{a}^{T}_{i}\Big{]}\Big{\\}}\\\
&=G_{i}F^{-1}G^{T}_{i}\end{split}$ (48)

where the identity that only one term in the former summation is nonzero based
on
$\text{E}\\{\boldsymbol{\Delta}\mathbf{a}_{j}\boldsymbol{\Delta}\mathbf{a}^{T}_{i}\\}=0$
for $i\neq j$ is used. This originates from the fact that different
measurements from non-identical features have zero correlation with each
other, which yields
$\text{E}\\{\boldsymbol{\Delta}\mathbf{r}_{j}\boldsymbol{\Delta}\mathbf{r}^{T}_{i}\\}=0$
and
$\text{E}\\{\boldsymbol{\Delta}\mathbf{b}_{j}\boldsymbol{\Delta}\mathbf{b}^{T}_{i}\\}=0$
for $i\neq j$.

####  3.2.3 Estimate Covariance for Measurements

The estimate covariance for observation vectors is defined as

$P_{\boldsymbol{\delta}\mathbf{d}_{i}}\equiv\text{cov}\\{\boldsymbol{\delta}\mathbf{d}_{i}\\}=\text{E}\\{\boldsymbol{\delta}\mathbf{d}_{i}\boldsymbol{\delta}\mathbf{d}^{T}_{i}\\}$
(49)

Using the first-order approximation in Eq. (43) gives

$\boldsymbol{\delta}\mathbf{d}_{i}\approx\boldsymbol{\Delta}\mathbf{d}_{i}+C_{i}(\boldsymbol{\Delta}\mathbf{a}_{i}-G_{i}\boldsymbol{\delta}\mathbf{x})$
(50)

The covariance within first order of the observation errors is

$\begin{split}P_{\boldsymbol{\delta}\mathbf{d}_{i}}&=\text{E}\Big{\\{}\Big{[}\boldsymbol{\Delta}\mathbf{d}_{i}+C_{i}(\boldsymbol{\Delta}\mathbf{a}_{i}-G_{i}\boldsymbol{\delta}\mathbf{x})\Big{]}\Big{[}\boldsymbol{\Delta}\mathbf{d}_{i}+C_{i}(\boldsymbol{\Delta}\mathbf{a}_{i}-G_{i}\boldsymbol{\delta}\mathbf{x})\Big{]}^{T}\Big{\\}}\\\
&=\text{E}\\{\boldsymbol{\Delta}\mathbf{d}_{i}\boldsymbol{\Delta}\mathbf{d}^{T}_{i}\\}+\text{cov}\\{\boldsymbol{\Delta}\mathbf{d}_{i}-\boldsymbol{\delta}\mathbf{d}_{i}\\}\\\
&+\text{E}\Big{\\{}C_{i}(\boldsymbol{\Delta}\mathbf{a}_{i}-G_{i}\boldsymbol{\delta}\mathbf{x})\boldsymbol{\Delta\mathbf{d}^{T}_{i}}\Big{\\}}+\text{E}\Big{\\{}C_{i}(\boldsymbol{\Delta}\mathbf{a}_{i}-G_{i}\boldsymbol{\delta}\mathbf{x})\boldsymbol{\Delta\mathbf{d}^{T}_{i}}\Big{\\}}^{T}\end{split}$
(51)

Then for the cross covariance terms, the following expression is given:

$\begin{split}\text{E}\Big{\\{}C_{i}(\boldsymbol{\Delta}\mathbf{a}_{i}-G_{i}\boldsymbol{\delta}\mathbf{x})\boldsymbol{\Delta}\mathbf{d}^{T}_{i}\Big{\\}}&=\text{E}\\{C_{i}\boldsymbol{\Delta}\mathbf{a}_{i}\boldsymbol{\Delta}\mathbf{d}^{T}_{i}\\}-\text{E}\\{C_{i}G_{i}\boldsymbol{\delta}\mathbf{x}\boldsymbol{\Delta}\mathbf{d}^{T}_{i}\\}\\\
&=\text{E}\\{-C_{i}S_{i}\boldsymbol{\Delta}\mathbf{d}_{i}\boldsymbol{\Delta}\mathbf{d}^{T}_{i}\\}-\text{E}\\{C_{i}G_{i}F^{-1}\sum_{j=1}^{n}G^{T}_{j}Q^{-1}_{\lambda_{j}}\boldsymbol{\Delta}\mathbf{a}_{j}\boldsymbol{\Delta}\mathbf{d}^{T}_{i}\\}\\\
&=-C_{i}S_{i}R_{i}+C_{i}G_{i}F^{-1}G^{T}_{i}C^{T}_{i}\end{split}$ (52)

where the identity of
$\boldsymbol{\Delta}\mathbf{a}_{i}=-S_{i}\boldsymbol{\Delta}\mathbf{d}_{i}$
and also the fact that only one term in the summation in Eq. (52) is nonzero
are used. Then from Eq. (18a) and Eq. (46), the covariance of measurement
estimate $P_{\boldsymbol{\delta}\mathbf{d}_{i}}$ will be

$\begin{split}P_{\boldsymbol{\delta}\mathbf{d}_{i}}&=R_{i}+C_{i}(Q_{\lambda_{i}}-G_{i}F^{-1}G^{T}_{i})C^{T}_{i}-(C_{i}S_{i}R_{i}+C^{T}_{i}S^{T}_{i}R^{T}_{i})+2C_{i}G_{i}F^{-1}G^{T}_{i}C^{T}_{i}\\\
&=R_{i}+C_{i}(Q_{\lambda_{i}}+G_{i}F^{-1}G^{T}_{i})C^{T}_{i}-(C_{i}S_{i}R_{i}+R^{T}_{i}S^{T}_{i}C^{T}_{i})\end{split}$
(53)

###  3.3 Fisher Information Matrix

The analytical derivations of the estimate covariances for the unknowns, i.e.
the attitude matrix $\hat{A}$, position vector $\hat{\mathbf{p}}$, depth
estimates $\hat{u_{i}}$ and $\hat{v_{i}}$, as well as the observation vectors
$\hat{\mathbf{r}}_{i}$ and $\hat{\mathbf{b}}_{i}$ have been provided in
section 3.2 with the small angle approximation. Now the optimality of the
estimates using the Fisher Information Matrix (FIM) and Cramér-Rao Lower Bound
(CRLB) [47] is shown. For an unbiased estimator $\mathbf{\hat{x}}$, the
estimate error-covariance has a lower bound as

$\text{cov}\\{\mathbf{\hat{x}}\\}\geq
F^{-1}\equiv\left(\text{E}\\{-\frac{\partial^{2}}{\partial\mathbf{\hat{x}}\partial\mathbf{\hat{x}}^{T}}p(\mathbf{\tilde{y}|\mathbf{\hat{x}}})\\}\right)^{-1}$
(54)

The term inside of the expectation shows the Hessian of the the negative log-
likelihood function, which is given in Eq. (32). For an efficient estimator,
the equality condition in Eq. (54) should be satisfied. The FIM is the Hessian
of the loss function in Eq. (32) with respect to the unknowns vector
$\boldsymbol{\delta}\mathbf{x}$ and is given by the term $F$ in Eq. (35a).
Note that from the derivation in Eq. (41), the covariance of the estimate is
equal to the inverse of the FIM which shows the optimality of the estimation
based on the equality condtion in Eq. (54).

##  4 Sensitivity Analysis

In this section, the sensitivity of the estimate errors and their
corresponding covariances with respect to the covariance of measurement noise
for virtual depths are checked. The term virtual is used here in a sense that
the actual depth data are not available in monocular camera images. Thus a
virtual depth measurement with a large covariance is instead used. Beginning
with the unknowns estimate vector $\boldsymbol{\delta}\mathbf{x}$, from Eq.
(36), gives

$\begin{split}\frac{\partial\boldsymbol{\delta}\mathbf{x}}{\partial
R_{u_{i}}}&=\frac{\partial F^{-1}\mathbf{g}}{\partial R_{u_{i}}}\\\
&=\big{[}\frac{\partial F^{-1}}{\partial
R_{u_{i}}}\big{]}\mathbf{g}+F^{-1}\frac{\partial\mathbf{g}}{\partial
R_{u_{i}}}\\\
&=-F^{-1}\frac{\mathbf{e}^{T}_{i}\mathbf{e}_{i}}{R^{2}_{u_{i}}}F^{-1}\mathbf{g}+F^{-1}\frac{\mathbf{e}^{T}_{i}\Delta
u_{i}}{R^{2}_{u_{i}}}\\\
&=\frac{F^{-1}\mathbf{e}^{T}_{i}}{R^{2}_{u_{i}}}\left(\Delta
u_{i}-\mathbf{e}_{i}\boldsymbol{\delta}\mathbf{x}\right)\end{split}$ (55)

Based on the definition of $\mathbf{e}_{i}$ in Eq. (33a) yields

$\frac{\partial\boldsymbol{\delta}\mathbf{x}}{\partial
R_{u_{i}}}=\frac{\text{cov}\\{\delta u_{i}\\}}{R^{2}_{u_{i}}}\left(\Delta
u_{i}-\delta u_{i}\right)$ (56)

This results in a positive number for the case of $\Delta u_{i}>\delta u_{i}$,
meaning that if the measurement error is larger than the estimate error, then
the estimate error of unknowns grows with an increase in the virtual depth
covariances. Also it is to be noted that the virtual depth measurements are
completely uncertain, i.e. $R_{u_{i}}=\infty$, then the derivative in Eq. (56)
is zero and the unknown error is independent of the covariance of virtual
depth measurements. Now the variations of unknowns estimate covariances with
the changes in the $R_{u_{i}}$ can be investigated. From Eqs. (41) and(33a),
the following expression is given:

$\begin{split}\frac{\partial\text{cov}\\{\boldsymbol{\delta}\mathbf{x}\\}}{\partial
R_{u_{i}}}&=\frac{\partial F^{-1}}{\partial R_{u_{i}}}\\\
&=F^{-1}\frac{\mathbf{e}^{T}_{i}\mathbf{e}_{i}}{R^{2}_{u_{i}}}F^{-1}\\\
&=\frac{\text{cov}\\{\delta u_{i}\\}}{R^{2}_{u_{i}}}\end{split}$ (57)

which indicates that in case of a very large covariance for $u_{i}$, the
derivative is approaching zero and the estimate covariance if not dependant on
the depth measurement covariances. This is intuitive since a depth measurement
data is not provided in monocular SLAM problems. For the covariance of the
residual errors for the observation unit vectors $\mathbf{r}_{i}$ and
$\mathbf{b}_{i}$, from Eq. (46), the following expression is given:

$\begin{split}\frac{\partial\text{cov}\\{\boldsymbol{\Delta}\mathbf{d}_{i}-\boldsymbol{\delta}\mathbf{d}_{i}\\}}{\partial
R_{u_{i}}}&=C_{i}G_{i}F^{-1}\frac{\mathbf{e}^{T}_{i}\mathbf{e}_{i}}{R^{2}_{u_{i}}}F^{-1}G^{T}_{i}C^{T}_{i}\\\
&=C_{i}G_{i}\frac{\text{cov}\\{\delta
u_{i}\\}}{R^{2}_{u_{i}}}F^{-1}G^{T}_{i}C^{T}_{i}\end{split}$ (58)

For the covariance estimates of the unit observation vectors, from Eq. (53),
the following expression is given:

$\begin{split}\frac{\partial P_{\boldsymbol{\delta}\mathbf{d}_{i}}}{\partial
R_{u_{i}}}&=-C_{i}G_{i}F^{-1}\frac{\mathbf{e}^{T}_{i}\mathbf{e}_{i}}{R^{2}_{u_{i}}}F^{-1}G^{T}_{i}C^{T}_{i}\\\
&=-C_{i}G_{i}\frac{\text{cov}\\{\delta
u_{i}\\}}{R^{2}_{u_{i}}}F^{-1}G^{T}_{i}C^{T}_{i}\end{split}$ (59)

The same analysis can be done for the covariance of virtual depth measurements
in the reference frame denoted by $R_{v_{i}}$. The other notable result is the
singularity of the FIM in case of increasing the covariance of the depth
measurements. For this purpose, the derivative of the determinant of the FIM
is calculated analytically with respect to the covariance of virtual
measurement:

$\begin{split}\frac{\partial|F|}{\partial
R_{u_{i}}}&=|F|\text{trace}\left(F^{-1}\frac{\partial F}{\partial
R_{u_{i}}}\right)\\\
&=|F|\text{trace}\left(\frac{-F^{-1}\mathbf{e}^{T}_{i}\mathbf{e}_{i}}{R^{2}_{u_{i}}}\right)\\\
&=-|F|\frac{\text{cov}\\{\delta u_{i}\\}}{R^{2}_{u_{i}}}\end{split}$ (60)

Since $\text{cov}\\{\delta u_{i}\\}$ and $R_{u_{i}}$ are positive definite,
and $|F|$ is a positive number, then the determinant is decreasing by making
the virtual depth more uncertain with a larger covariance. This means that the
FIM gets closer to being singular when increasing the uncertainty in the
virtual depth. Also, the more information that is known about the depth, the
better conditioned the FIM becomes. In a real application, the virtual depths
have a very large covariance and there is no certainty about them, which makes
the FIM close to singular. This results in a large $3\sigma$ bound for the
position and depth errors, as will been seen in the Monte Carlo analysis in
section 5.

##  5 Numerical Validation in Monte-Carlo Simulation

(a) Attitude Errors

(b) Position Errors

Figure 2: Monte Carlo simulation for the attitude and translation vector.

(a) Estimation Errors

(b) Residual Errors

Figure 3: Monte Carlo simulation for for estimates and residuals of the vector
observation $\mathbf{r}_{1}$.

(a) Estimation Errors

(b) Residual Errors

Figure 4: Monte Carlo simulation for for estimates and residuals of the vector
observation $\mathbf{b}_{1}$. Figure 5: Monte Carlo simulation for estimates
errors of virtual depth $u_{i}$, $i=1,...,n$. Figure 6: Monte Carlo simulation
for estimates errors of virtual depth $v_{i}$, $i=1,...,n$. Figure 7: The
effect of virtual depth covariance on the singularity of the FIM.

A pose estimation problem is solved here with two scans of a monocular camera
image using the unit-sphere sensor projection model. The minimum number of
features from the simulations is three. Using only three features, however,
does not provide a good estimate in terms of the attitude error amplitudes,
although the estimate errors are completely covered by the $3\sigma$ bounds
from the analytical covariance derivations. In the simulations six features
per image with two images are used to estimate the pose unknowns. The ground
truth values for the attitude matrix $A$, translation vector $\mathbf{p}$ and
vector observations $\mathbf{b}_{i}$, $i=1,\,2,\,3$, are given as

$\displaystyle A=\begin{bmatrix}\cos{\frac{\pi}{4}}&\sin{\frac{\pi}{4}}&0\\\
-\sin{\frac{\pi}{4}}&\cos{\frac{\pi}{4}}&0\\\ 0&0&1\end{bmatrix}$
$\displaystyle\mathbf{p}=\begin{bmatrix}0.7512&1.7783&1.2231\end{bmatrix}^{T}$
$\displaystyle\mathbf{r}_{1}=\begin{bmatrix}0.6930&-0.0639&0.7181\end{bmatrix}^{T}\text{,\
}\mathbf{r}_{2}=\begin{bmatrix}0.5074&0.8032&0.3120\end{bmatrix}^{T}\text{,\
}\mathbf{r}_{3}=\begin{bmatrix}0.1558&0.0360&0.9871\end{bmatrix}^{T}$
$\displaystyle\mathbf{r}_{4}=\begin{bmatrix}-0.4723&-0.7507&-0.4618\end{bmatrix}^{T}\text{,\
}\mathbf{r}_{5}=\begin{bmatrix}-0.9202&-0.3649&-0.1418\end{bmatrix}^{T}\text{,\
}\mathbf{r}_{6}=\begin{bmatrix}-0.3115&0.7715&0.5548\end{bmatrix}^{T}$
$\displaystyle\mathbf{b}_{1}=\begin{bmatrix}0.3037&-0.6373&0.7082\end{bmatrix}^{T}\text{,\
}\mathbf{b}_{2}=\begin{bmatrix}0.9562&-0.0606&0.2863\end{bmatrix}^{T}\text{,\
}\mathbf{b}_{3}=\begin{bmatrix}0.1102&-0.1204&0.9866\end{bmatrix}^{T}$
$\displaystyle\mathbf{b}_{4}=\begin{bmatrix}-0.8856&0.0003&-0.4645\end{bmatrix}^{T}\text{,\
}\mathbf{b}_{5}=\begin{bmatrix}-0.8020&0.5776&-0.1524\end{bmatrix}^{T}\text{,\
}\mathbf{b}_{6}=\begin{bmatrix}0.4981&0.6662&0.5551\end{bmatrix}^{T}$
$\displaystyle{u}_{1}=125.1371\text{,\ \ }{u}_{2}=35.1825\text{,\ \
}{u}_{3}=281.2848\text{,\ \ }{u}_{4}=248.2209\text{,\ \
}{u}_{5}=118.5592\text{,\ \ }{u}_{6}=67.9268$
$\displaystyle{v}_{1}=125.1189\text{,\ \ }{v}_{2}=36.2025\text{,\ \
}{v}_{3}=282.3673\text{,\ \ }{v}_{4}=246.9957\text{,\ \
}{v}_{5}=118.8191\text{,\ \ }{v}_{6}=70.1661$

The other $u_{i}\text{\ , i}=1,...,n$, and $v_{i}\text{\ , i}=1,...,n$ values
are generated by normal random vectors. The standard deviation for the random
generator of $\mathbf{r}_{i}\text{\ , i}=1,...,n$ is 100 meters. The true
observation vectors $\mathbf{b}_{i}$ are generated by the constraint in Eq.
(9) and $u_{i}$’s are the norms of the projection vectors from the camera
center of projection towards the feature in the environment. A Monte-Carlo
simulation with 10,000 samples is performed here to showcase how well the
$3\sigma$ bounds generated by the covariance expressions in Eqs. (41) for the
attitude and position and virtual depths, cover the amplitude of the estimate
errors. Also the $3\sigma$ bounds in Eq. (46) and Eq. (53) are checked to
cover their corresponding residuals and estimate errors. Artificial noise is
generated from a Gaussian distribution with zero mean and covariance of
$R_{r_{i}}$, $R_{b_{i}}$, $R_{u_{i}}$, $R_{v_{i}}$ to produce
$\tilde{\mathbf{r}}_{i}$, $\tilde{\mathbf{b}}_{i}$, $\tilde{u}_{i}$, and
$\tilde{v}_{i}$ samples. The covariance matrices of the measurements
$R_{r_{i}}$, $R_{b_{i}}$ are generated randomly with a coefficient of $0.006$
degrees, multiplied by a random $3\times 3$ matrix, and finally multiplied by
its transpose to generate a positive definite matrix. Also the standard
deviation of the depth covariances is $\epsilon_{uv}=190$ meters, which is
multiplied by the absolute value of a normal Gaussian random number. The
covariances of the virtual depths are

$\begin{split}R_{u_{1}}=1.5609\times 10^{4}\text{,\ }R_{u_{2}}=1.2334\times
10^{4}\text{,\ }R_{u_{3}}=1.2882\times 10^{4}\\\ R_{u_{4}}=9.9700\times
10^{4}\text{,\ }R_{u_{5}}=4.8596\times 10^{4}\text{,\ }R_{u_{6}}=1.0926\times
10^{5}\\\ R_{v_{1}}=1.9356\times 10^{4}\text{,\ }R_{v_{2}}=6.2020\times
10^{4}\text{,\ }R_{v_{3}}=8.1318\times 10^{4}\\\ R_{v_{4}}=3.1038\times
10^{4}\text{,\ }R_{v_{5}}=1.1476\times 10^{4}\text{,\ }R_{v_{6}}=4.7077\times
10^{4}\\\ \end{split}$ (61)

The covarince of line-of-sight unit vectors are

$\begin{split}&R_{r_{1}}=10^{-8}\times\begin{bmatrix}4.04&2.53&-0.335\\\
2.53&10.1&-5.75\\\ -0.335&-5.75&4.39\end{bmatrix}\text{,\
}R_{b_{1}}=10^{-8}\times\begin{bmatrix}1.42&1.44&-1.35\\\ 1.44&1.48&-1.46\\\
-1.35&-1.46&2.07\end{bmatrix}\\\
&R_{r_{2}}=10^{-8}\times\begin{bmatrix}0.15&-0.20&0.36\\\ -0.20&4.24&-0.08\\\
0.36&-0.08&3.37\end{bmatrix}\text{,\
}R_{b_{2}}=10^{-8}\times\begin{bmatrix}5.06&3.03&1.18\\\ 3.03&3.22&1,02\\\
1.18&1.02&1.57\end{bmatrix}\\\
&R_{r_{3}}=10^{-8}\times\begin{bmatrix}8.25&0.41&1.33\\\ 0.41&3.50&-2.01\\\
1.33&-2.01&1.51\end{bmatrix}\text{,\
}R_{b_{3}}=10^{-8}\times\begin{bmatrix}0.83&-0.51&-0.50\\\ -0.51&0.61&0.67\\\
-0.50&0.67&5.73\end{bmatrix}\\\
&R_{r_{4}}=10^{-8}\times\begin{bmatrix}8.84&-2.08&-0.06\\\ -2.08&0.55&-0.32\\\
-0.06&-0.32&2.10\end{bmatrix}\text{,\
}R_{b_{4}}=10^{-8}\times\begin{bmatrix}3.55&-1.53&1.82\\\ -1.53&5.19&-0.06\\\
-1.82&-0.06&1.17\end{bmatrix}\\\
&R_{r_{5}}=10^{-8}\times\begin{bmatrix}4.38&-1.93&-3.51\\\ -1.93&2.31&-0.74\\\
-3.51&-0.74&6.87\end{bmatrix}\text{,\
}R_{b_{5}}=10^{-8}\times\begin{bmatrix}14.5&3.18&8.50\\\ 3.18&0.80&1.70\\\
8.50&1.70&5.94\end{bmatrix}\\\
&R_{r_{6}}=10^{-8}\times\begin{bmatrix}7.16&1.00&3.01\\\ 1.00&0.46&0.24\\\
3.01&0.24&1.36\end{bmatrix}\text{,\
}R_{b_{6}}=10^{-8}\times\begin{bmatrix}2.86&-1.04&-0.43\\\ -1.04&2.33&-1.49\\\
-0.43&-1.49&1.48\end{bmatrix}\end{split}$ (62)

and then $R_{r_{i}}\text{\ , i}=1,...,n$ and $R_{b_{i}}\text{\ , i}=1,...,n$
are concatenated using Eq. (18a). Note that by default the measurement
covariance is selected to be positive definite while being random.
Singularities in the measurement covariance matrix can be handled if they
exist, but this is not the focus of this paper.

Figure 2(a) shows the plot of the attitude errors in terms of roll, pitch and
yaw angles in degrees from the Monte-Carlo samples. The blue line depicts the
estimation errors, and the red lines are the $3\sigma$ bounds computed from
the estimate error-covariances. Figure 2(b) shows the translation vector
estimate error in the $x$, $y$ and $z$ directions, respectively. It is seen
that the estimate errors are well-bounded by their corresponding $3\sigma$
bounds. Figure 4(a) shows the estimation errors, and Figure 4(b) shows the
residual errors, for observation vector $\mathbf{b}_{1}$. Figures 3(a) and
3(b) show the same results for the observation vector $\mathbf{r}_{1}$,
respectively. It is seen that the observation vectors are also bounded by
their corresponding $3\sigma$ bounds, provided by the covariances of estimates
as well as the residuals. For the virtual depth vectors, figure 5 and 6 show
the estimate errors versus their corresponding analytical covariace, which
shows the $3\sigma$ bounds covering the estimate errors. Note that the
estimate covariances for the virtual depths are much better than the initial
covariance in Eq. (61).

The effect of the virtual depth standard deviation $\epsilon_{uv}$ on the
sigularity of the FIM is shown in figure 7. It is seen that by increasing
$\epsilon_{uv}$, the reciprocal condition number of the FIM decreases, which
means that the determinant is decreasing and the condition number is
increasing. The FIM is pushed more towards singularity which is intuitive, the
more information that is given about the virtual depth, more accurate
estimates are obtained and the FIM becomes less singular.

##  6 Conclusion

This study provides an analytical framework for an optimal estimator of
monocular pose estimation, which is central to the SLAM problem. The static
SLAM problem is shown to be solved as a total least squares problem. A
quadratic cost function based on the TLS formulation is introduced for taking
into account the attitude matrix, the translation vector and the virtual depth
measurements with large covariance as the set of unknowns for this problem.
The weight matrix in the cost function is extracted from the most generic
positive-definite fully populated matrix to include the correlations between
the observation vectors in the most general case. The cost function is then
written in terms of the vector of concatenated unknowns, and the covariance
expression for the attitude error is provided alongside other unknowns within
the small-angle assumption and within a second-order approximation of the cost
function in terms of the unknowns estimate error. Optimal estimation of the
unknowns makes the controller policy easier to track the desired signals in
terms of obtaining a more accurate estimate of the states, which in turn
consume lower levels of energy in the control action. The virtual measurement
are employed to avoid the observability problems. Covariance expressions for
the translation vector, position vector, virtual depths, and the estimates and
residuals of the observation line-of-sight unit vectors, are obtained
analytically. The Fisher information matrix is derived, and the covariance
expression of unknowns is proven to be inverse of it, which proves the
equality in the Cramér-Rao lower bound, and thus in the optimality of the
estimates. A sensitivity analysis for the estimate errors and covariances is
developed based on the perturbations in the virtual depth covariances. This
analysis shows that for the large values of the depth covariance, the estimate
covariances of unknowns are independent of the depth covariance which agrees
with the fact that the depth data are not available in monocular pose
estimation problem. A simulation framework showcases the efficacy of the
covariance analyses by simulating observation vectors in a pose estimation
problem with 10,000 Monte-Carlo samples.

## References

  * Markley and Crassidis [2014] Markley, F. L., and Crassidis, J. L., _Fundamentals of Spacecraft Attitude Determination and Control_ , Springer, 2014.
  * Crassidis et al. [2007] Crassidis, J. L., Markley, F. L., and Cheng, Y., “Survey of Nonlinear Attitude Estimation Methods,” _Journal of Guidance, Control, and Dynamics_ , Vol. 30, No. 1, 2007, pp. 12–28.  10.2514/1.22452.
  * Psiaki et al. [1990] Psiaki, M. L., Martelt, F., and Pal, P. K., “Three-Axis Attitude Determination via Kalman Filtering of Magnetometer Data,” _Journal of Guidance, Control, and Dynamics_ , Vol. 13, No. 3, 1990, pp. 506–514.  10.2514/3.25364.
  * Wahba [1965] Wahba, G., “A Least Squares Estimate of Satellite Attitude,” _SIAM Review_ , Vol. 7, No. 3, 1965, pp. 409–409.  10.1137/1008080.
  * Markley and Mortari [2000] Markley, F. L., and Mortari, D., “Quaternion Attitude Estimation Using Vector Observations,” _The Journal of the Astronautical Sciences_ , Vol. 48, No. 2, 2000, pp. 359–380.  10.1007/BF03546284.
  * Eggert et al. [1997] Eggert, D. W., Lorusso, A., and Fisher, R. B., “Estimating 3-D Rigid Body Transformations: A Comparison of Four Major Algorithms,” _Machine Vision and Applications_ , Vol. 9, No. 5, 1997, pp. 272–290.  10.1007/s001380050048.
  * Kelsey et al. [2006] Kelsey, J. M., Byrne, J., Cosgrove, M., Seereeram, S., and Mehra, R. K., “Vision-Based Relative Pose Estimation for Autonomous Rendezvous and Docking,” _2006 IEEE Aerospace Conference_ , IEEE, 2006.  10.1109/AERO.2006.1655916.
  * Jurie et al. [2002] Jurie, F., Dhome, M., et al., “Real Time Robust Template Matching,” _BMVC_ , Vol. 2002, 2002, pp. 123–132.  10.5244/C.16.10.
  * Isard and Blake [1998] Isard, M., and Blake, A., “Condensation—Conditional Density Propagation for Visual Tracking,” _International Journal of Computer Vision_ , Vol. 29, No. 1, 1998, pp. 5–28.  10.1023/A:1008078328650.
  * Schonberger and Frahm [2016] Schonberger, J. L., and Frahm, J.-M., “Structure-From-Motion Revisited,” _Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition_ , 2016, pp. 4104–4113.  10.1109/CVPR.2016.445.
  * Rehbinder and Ghosh [2003] Rehbinder, H., and Ghosh, B. K., “Pose Estimation Using Line-based Dynamic Vision and Inertial Sensors,” _IEEE Transactions on Automatic Control_ , Vol. 48, No. 2, 2003, pp. 186–199.  10.1109/TAC.2002.808464.
  * Sharma and D’Amico [2016] Sharma, S., and D’Amico, S., “Comparative Assessment of Techniques for Initial Pose Estimation Using Monocular Vision,” _Acta Astronautica_ , Vol. 123, 2016, pp. 435–445.  10.1016/j.actaastro.2015.12.032.
  * Lasenby et al. [1998] Lasenby, J., Fitzgerald, W. J., Lasenby, A. N., and Doran, C., “New Geometric Methods for Computer Vision: An Application to Structure and Motion Estimation,” _International Journal of Computer Vision_ , Vol. 26, No. 3, 1998, pp. 191–213.  10.1023/A:1007901028047.
  * Huang and Netravali [2002] Huang, T. S., and Netravali, A. N., “Motion and Structure From Feature Correspondences: A Review,” _Advances In Image Processing And Understanding: A Festschrift for Thomas S Huang_ , 2002, pp. 331–347.  10.1142/9789812776952_0013.
  * Fu et al. [2017] Fu, Q., Quan, Q., and Cai, K.-Y., “Robust Pose Estimation for Multirotor UAVs Using Off-board Monocular Vision,” _IEEE Transactions on Industrial Electronics_ , Vol. 64, No. 10, 2017, pp. 7942–7951.  10.1109/TIE.2017.2696482.
  * Melekhov et al. [2017] Melekhov, I., Ylioinas, J., Kannala, J., and Rahtu, E., “Relative Camera Pose Estimation Using Convolutional Neural Networks,” _International Conference on Advanced Concepts for Intelligent Vision Systems_ , Springer, 2017, pp. 675–687.  10.1007/978-3-319-70353-4_57.
  * Li et al. [2018] Li, Y., Wang, G., Ji, X., Xiang, Y., and Fox, D., “Deepim: Deep Iterative Matching for 6d Pose Estimation,” _Proceedings of the European Conference on Computer Vision (ECCV)_ , 2018, pp. 683–698.  10.1007/s11263-019-01250-9.
  * Toshev and Szegedy [2014] Toshev, A., and Szegedy, C., “Deeppose: Human Pose Estimation via Deep Neural Networks,” _Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition_ , 2014, pp. 1653–1660.  10.1109/CVPR.2014.214.
  * Zhang et al. [2010] Zhang, S., Cao, X., Zhang, F., and He, L., “Monocular Vision-based Iterative Pose Estimation Algorithm From Corresponding Feature Points,” _Science China Information Sciences_ , Vol. 53, No. 8, 2010, pp. 1682–1696.  10.1007/s11432-010-4017-6.
  * Dornaika and Garcia [1999] Dornaika, F., and Garcia, C., “Pose Estimation Using Point and Line Correspondences,” _Real-Time Imaging_ , Vol. 5, No. 3, 1999, pp. 215–230.  10.1006/rtim.1997.0117.
  * Erol et al. [2007] Erol, A., Bebis, G., Nicolescu, M., Boyle, R. D., and Twombly, X., “Vision-Based Hand Pose Estimation: A Review,” _Computer Vision and Image Understanding_ , Vol. 108, No. 1, 2007, pp. 52–73.  10.1016/j.cviu.2006.10.012.
  * Forsyth and Ponce [2012] Forsyth, D., and Ponce, J., _Computer Vision: A Modern Approach_ , Vol. 2, 2012\.
  * Dhanachandra et al. [2015] Dhanachandra, N., Manglem, K., and Chanu, Y. J., “Image Segmentation Using K-means Clustering Algorithm and Subtractive Clustering Algorithm,” _Procedia Computer Science_ , Vol. 54, 2015, pp. 764–771.  10.1016/j.procs.2015.06.090.
  * Chen et al. [2017] Chen, L.-C., Papandreou, G., Schroff, F., and Adam, H., “Rethinking Atrous Convolution for Semantic Image Segmentation,” _arXiv preprint arXiv:1706.05587_ , 2017.
  * Plath et al. [2009] Plath, N., Toussaint, M., and Nakajima, S., “Multi-Class Image Segmentation Using Conditional Random Fields and Global Classification,” _Proceedings of the 26th Annual International Conference on Machine Learning_ , 2009, pp. 817–824.  10.1145/1553374.1553479.
  * Minaee et al. [2021] Minaee, S., Boykov, Y. Y., Porikli, F., Plaza, A. J., Kehtarnavaz, N., and Terzopoulos, D., “Image Segmentation Using Deep Learning: A Survey,” _IEEE Transactions on Pattern Analysis and Machine Intelligence_ , 2021.  0.1109/TPAMI.2021.3059968.
  * Chouhan et al. [2018] Chouhan, S. S., Kaul, A., and Singh, U. P., “Soft Computing Approaches for Image Segmentation: A survey,” _Multimedia Tools and Applications_ , Vol. 77, No. 21, 2018, pp. 28483–28537.  10.1007/s11042-018-6005-6.
  * Zaitoun and Aqel [2015] Zaitoun, N. M., and Aqel, M. J., “Survey on Image Segmentation Techniques,” _Procedia Computer Science_ , Vol. 65, 2015, pp. 797–806.  10.1016/j.procs.2015.09.027.
  * Canny [1986] Canny, J., “A Computational Approach to Edge Detection,” _IEEE Transactions on Pattern Analysis and Machine Intelligence_ , Vol. PAMI-8, No. 6, 1986, pp. 679–698.  10.1109/TPAMI.1986.4767851.
  * Lowe [2004] Lowe, D. G., “Distinctive Image Features from Scale-invariant Keypoints,” _International Journal of Computer Vision_ , Vol. 60, No. 2, 2004, pp. 91–110.  10.1023/B:VISI.0000029664.99615.94.
  * Bay et al. [2006] Bay, H., Tuytelaars, T., and Van Gool, L., “Surf: Speeded Up Robust Features,” _European Conference on Computer Vision_ , Springer, 2006, pp. 404–417.  10.1007/11744023_32.
  * [32] Hough, P. V., “Method and Means for Recognizing Complex Patterns,” , ????  US Patent 3,069,654, Dec. 18, 1962.
  * Ballard [1981] Ballard, D. H., “Generalizing the Hough Transform to Detect Arbitrary Shapes,” _Pattern Recognition_ , Vol. 13, No. 2, 1981, pp. 111–122.  10.1016/0031-3203(81)90009-1.
  * Salahat and Qasaimeh [2017] Salahat, E., and Qasaimeh, M., “Recent Advances in Features Extraction and Description Algorithms: A Comprehensive Survey,” _2017 IEEE International Conference on Industrial Technology (ICIT)_ , IEEE, 2017, pp. 1059–1063.  10.1109/ICIT.2017.7915508.
  * Davison et al. [2007] Davison, A. J., Reid, I. D., Molton, N. D., and Stasse, O., “MonoSLAM: Real-time Single Camera SLAM,” _IEEE Transactions on Pattern Analysis and Machine Intelligence_ , Vol. 29, No. 6, 2007, pp. 1052–1067.  10.1109/TPAMI.2007.1049.
  * Bian et al. [2017] Bian, J., Lin, W.-Y., Matsushita, Y., Yeung, S.-K., Nguyen, T.-D., and Cheng, M.-M., “GMS: Grid-based Motion Statistics for Fast, Ultra-Robust Feature Correspondence,” _Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition (CVPR)_ , 2017.  10.1007/s11263-019-01280-3.
  * Zhao et al. [2019] Zhao, C., Yang, J., Xiao, Y., and Cao, Z., “Comparative Evaluation of 2D Feature Correspondence Selection Algorithms,” _arXiv preprint arXiv:1904.13383_ , 2019.
  * Hashim [2020] Hashim, H. A., “Attitude Determination and Estimation Using Vector Observations: Review, Challenges and Comparative Results,” _arXiv preprint arXiv:2001.03787_ , 2020.
  * Crassidis and Junkins [2012] Crassidis, J. L., and Junkins, J. L., _Optimal Estimation of Dynamic Systems_ , 2nd ed., Chapman and Hall/CRC, Boca Raton, Fl, 2012, Chap. 2.  10.1201/b11154.
  * Maleki et al. [2022] Maleki, S., Crassidis, J. L., Cheng, Y., and Schmid, M., “Total Least Squares for Optimal Pose Estimation,” _AIAA Scitech 2022 Forum_ , AIAA, Reston, VA, 2022.  10.2514/6.2022-1222.
  * Maleki [2022] Maleki, S., “Thesis: Total Least Squares for Optimal Pose Estimation,” _ProQuest Dissertations Publishing_ , 2022.
  * Cheng and Crassidis [2021] Cheng, Y., and Crassidis, J. L., “Optimal Pose Estimation with Error-Covariance Analysis,” _AIAA Scitech 2021 Forum_ , 2021.  10.2514/6.2021-1758.
  * Crassidis and Cheng [2019] Crassidis, J. L., and Cheng, Y., “Maximum Likelihood Analysis of the Total Least Squares Problem with Correlated Errors,” _Journal of Guidance, Control, and Dynamics_ , Vol. 42, No. 6, 2019, pp. 1204–1217.  10.2514/6.2019-1931.
  * Golub [1973] Golub, G. H., “Some Modified Matrix Eigenvalue Problems,” _SIAM Review_ , Vol. 15, No. 2, 1973, pp. 318–334.  10.1137/1015032.
  * Markovsky and Van Huffel [2007] Markovsky, I., and Van Huffel, S., “Overview of Total Least-Squares Methods,” _Signal Processing_ , Vol. 87, No. 10, 2007, pp. 2283–2302.  10.1016/j.sigpro.2007.04.004.
  * Golub and Van Loan [1980] Golub, G. H., and Van Loan, C. F., “An Analysis of the Total Least Squares Problem,” _SIAM Journal on Numerical Analysis_ , Vol. 17, No. 6, 1980, pp. 883–893.  10.1137/0717073.
  * Cramér [1999] Cramér, H., _Mathematical Methods of Statistics_ , Vol. 43, Princeton University Press, 1999.  10.1515/9781400883868.