# Uncertainty analysis for accurate ground truth trajectories with robotic
total stations

Maxime Vaidis1, William Dubois, Effie Daum, Damien LaRocque, François
Pomerleau1 1Northern Robotics Laboratory, Université Laval, Québec City,
Canada, $\\{$maxime.vaidis, francois.pomerleau$\\}$ @norlab.ulaval.ca

###### Abstract

In the context of robotics, accurate ground truth positioning is essential for
the development of Simultaneous Localization and Mapping (SLAM) and control
algorithms. Robotic Total Stations provide accurate and precise reference
positions in different types of outdoor environments, especially when compared
to the limited accuracy of Global Navigation Satellite System (GNSS) in
cluttered areas. Three RTS give the possibility to obtain the six- Degrees Of
Freedom (DOF) reference pose of a robotic platform. However, the uncertainty
of every pose is rarely computed for trajectory evaluation. As evaluation
algorithms are getting increasingly precise, it becomes crucial to take into
account this uncertainty. We propose a method to compute this six-DOF
uncertainty from the fusion of three RTS based on Monte Carlo (MC) methods.
This solution relies on point-to-point minimization to propagate the noise of
RTS on the pose of the robotic platform. Five main noise sources are
identified to model this uncertainty: noise inherent to the instrument, tilt
noise, atmospheric factors, time synchronization noise, and extrinsic
calibration noise. Based on extensive experimental work, we compare the impact
of each noise source on the prism uncertainty and the final estimated pose.
Tested on more than $50\text{\,}\mathrm{k}\mathrm{m}$ of trajectories, our
comparison highlighted the importance of the calibration noise and the
measurement distance, which should be ideally under $75\text{\,}\mathrm{m}$.
Moreover, it has been noted that the uncertainty on the pose of the robot is
not prominently affected by one particular noise source, compared to the
others.

##  I Introduction

In mobile robotics, the current development of mapping and control algorithms
heavily relies on datasets [1]. The performance of these algorithms is
evaluated by comparing the different poses with a reference trajectory. In
outdoor environments, RTS provide the highest accuracy by measuring reference
trajectories with uncertainty on the position in the range of millimeters [2].
Coming from the field of surveying, a _total station_ is an optic-based
measurement instrument that can be precisely aimed at a given prismatic retro-
reflector (i.e., simply called _prism_ in the remainder of this article). A
total station is _robotic_ when it can automatically track a prism, while this
prism is in motion. The position of the prism is computed in the local
coordinate system of the RTS, according to the horizontal and vertical angles,
along with the range between the RTS and the measured prism. With three prisms
or more attached to a robotic platform, it is possible to compute its six-DOF
pose through manual static measurement [3] or through the use of multiple RTS
continuously tracking three _active prisms_ rigidly mounted on the same
platform [4]. Active prisms are recently available off the shelf and provide a
unique light signature for automatic target identification by RTS. Each prism
is tracked by its own assigned RTS, as shown in Figure 1. The distance between
a RTS and its prism is determined by Electronic optical Distance Measurement
(EDM), which is greatly impacted by weather conditions [5].

Figure 1: Setup used to record a reference trajectory during a snowstorm.
Three RTS are each tracking a specific active prism, all mounted on a
Clearpath Warthog robotic platform.

Yet, to be useful in autonomous navigation research, evaluation protocols need
to be used in a variety of conditions and environments, such as snowfalls [6],
which can increase the uncertainty of ground truth trajectories. In addition
to measurement noise inherent to a single RTS, using multiple RTS involves
time synchronization and extrinsic calibration to fuse the data of all RTS in
a common frame [7]. Both this synchronization and calibration carry noises and
uncertainties that must be studied. Uncertainty analysis is not usually part
of the SLAM algorithms evaluation pipelines, as the most common metric for
comparing trajectories is the Euclidean distance. However, as current
algorithms are developed with the intention to be accurate, an evaluation that
does not consider uncertainties will lead to biased results. Moreover,
uncertainty estimations for ground truth trajectories are missing in state-of-
the-art outdoor SLAM datasets. Two factors could explain this: 1) Since ground
truth trajectory noises are considered negligible, uncertainty is not
computed. 2) It can be complex to model the uncertainty of a reference system,
such as GNSSs. Uncertainty models were developed for RTS [8], but they were
never used for trajectory evaluations on mobile robots driven in outdoor
environments.

This work is based on our previous research to develop an RTS setup for
trajectory evaluations [4, 7]. In this paper, we propose a method to model RTS
uncertainties with the objective to better compare six-DOF trajectories. For
that purpose, we carried out a MC method that includes five different sources
of uncertainty relative to multiple RTS measurements in outdoor environments.
These uncertainties are then interpolated over time by a Gaussian Process (GP)
and propagated to the reference pose of a robot by using another MC method. A
detailed qualitative analysis of the different noise sources is presented, as
well as their impact on the final resulting six-DOF trajectories. The
experimental data used to compute the results was gathered during a whole year
of deployments, with over $50\text{\,}\mathrm{km}$ of recorded trajectories in
different weather conditions and environments. Both our source code and our
dataset are freely available in our RTS_Project
repository.111https://github.com/norlab-ulaval/RTS_project

##  II Related work

We first describe the current uses of RTS to obtain reference trajectories for
mobile robotics. Then, we present different studies of RTS-related
uncertainties and we expose different methods that are used in the state of
the art to model and propagate uncertainties. Finally, we address the use of
these methods in mobile robotics and we discuss their properties.

RTS-based positioning systems are quite common in mobile robotics. The number
of RTS in an experimental setup is determined by the number of prisms that can
be handled by the robotic platform, as well as the number of DOFs in the
desired resulting trajectory. A single RTS was used to acquire the three-DOF
position of a prism mounted on different robotic platforms, such as a
planetary rover [9], a tracked robot [10], an unmanned surface vessel [11], a
skid steered robot [12], and a Unmanned Aerial Vehicle (UAV) [13]. It is
possible to reduce the uncertainty of the reference position by adding a
second RTS to track the same prism. [14] have used a second RTS to follow two
different prisms on a compost turner, enabling the measurement of four DOFs on
the platform (i.e., the position and the yaw angle). To obtain the full pose
reference of a static robotic platform, it is possible to manually measure
three prisms with a single RTS [3]. Furthermore, this setup provides a
quantitative way to analyze uncertainty through inter-prism distances. These
distances can be compared with values that were accurately determined in a
controlled environment. For a moving platform, [4] developed the first method
to compute and interpolate six-DOF poses of a robot, with the measurements of
three RTS. In this paper, we build on this method by providing a continuous
six-DOF pose uncertainty model that relies on RTS measurements.

Many noise sources can be modeled and used to estimate the uncertainty of a
RTS’s measurement. Each noise model has an impact on different parts of a RTS
processing pipeline, from the raw measurements of the RTS to the estimated
Cartesian position of a prism. Most uncertainty sources are directly related
to the devices (measuring instruments and prisms). Distances and angles
uncertainties can be estimated with manufacturer’s specifications, or with
experimental results, done both in laboratories. Outside these controlled
environments, the atmospheric factors (e.g., temperature, pressure, and
humidity) need to be considered, due to EDM sensitivity [5]. As such, a
variation on $1\text{\,}\mathrm{\SIUnitSymbolCelsius}$ can lead to an error of
$0.2\text{\,}\mathrm{mm}$ on a measured distance of $200\text{\,}\mathrm{m}$
[15]. The noise of a Robotic Total Station’s electronic compensator can be
estimated through manufacturer specifications, yet the associated uncertainty
is often disregarded when conducting precise surveying [16]. Moreover, time
synchronization errors and uncertainties can occur in the communication
between a RTS and an external controller or data acquisition system [17]. When
using multiple RTS, the accuracy of the extrinsic calibration between all RTS
influences the accuracy of the estimated prism positions. [7] implemented a
pipeline to filter outliers on RTS data and proposed an extrinsic calibration
method that corrects the error on the poses, yet uncertainty remained.
Finally, a moving target creates some additional uncertainties that are
difficult to quantify. This noise comes from the limitations of the RTS
angular tracking system, especially at high prism speeds and accelerations
[18]. When using multiple prisms, the inter-prism distances can be used to
filter imprecise results with a threshold on prism speeds [4]. This paper
examines all these sources of uncertainty, to model the global uncertainty of
each RTS measurement under different atmospheric conditions.

There are two main ways to model the total uncertainty of a RTS, based on the
aforementioned sources of uncertainty: either with an approach that is based
on the Guide to the expression of Uncertainty in Measurement (GUM), or with MC
simulations. The Guide to the expression of Uncertainty in Measurement (GUM)
[19] divides uncertainties into two types, between those obtained from
statistical analysis on a series of observations (defined as _Type A_), and
those expressed by average manufacturer-specified or user-defined values
(defined as _Type B_). With the GUM method, an uncertainty budget of a RTS
allows one to express the total uncertainty of this RTS as an isotropic noise
[20]. This method works well for noise sources that can be linearized, but it
can be complex to implement for non-linear noise, such as weather conditions.
For this reason, MC simulations are widely used to determine the uncertainty
of RTS, whether for simple models [21], or very complex models taking into
account non-linear noise, such as atmospheric factors [8]. Moreover, the
resulting uncertainty is modeled as anisotropic. Generally, a MC method relies
on between $10^{3}$ to $10^{5}$ samples to have coherent generated results,
making this method computationally greedy for large datasets [22]. Both of
these methods give an estimate of a prism’s position uncertainty, yet it is
unsuitable for mobile robotics when the uncertainty is propagated into the
reference frame of a robotic platform. Several algorithms exist to propagate
uncertainty in a system. An Unscented Kalman filter can be used to estimate
the resulting noise [23]. The Unscented transform method has been carried out
to tackle computational resources issues, with as accurate results for
uncertainty estimation as with MC [24]. This method is based on the key idea
that it should be easier to approximate a probability distribution than to
approximate an arbitrary nonlinear function. Yet, the Unscented transform is
only applied to points of a specific covariance distribution at a time. Since
three-RTS positioning systems give three different covariance distributions,
other methods that can process multiple distributions are more appropriate.
Other studies have used Lie Algebra to link and interpolate the pose of a
system to its uncertainty. [25] formalized ways to work with noise in
$\mathfrak{se}(3)$ and applied them to propagate the noise from a camera over
a trajectory. [26] developed a library called Simultaneous Trajectory
Estimation And Mapping (STEAM) that uses GP to interpolate the covariance
matrix of a system for nonlinear optimization problems with continuous-time
components. In this article, we combine the research of [8] and [26] to
propagate the uncertainty to the pose of a robotic platform.

##  III Theory

We first present our approach for modeling uncertainty of RTS measurements
with the MC method. Then, we show how we interpolate data with GP for prism
measurement uncertainties. Next, we describe how we use another Monte Carlo
method to propagate uncertainty from interpolated prism measurements to six-
DOF vehicle poses.

###  III-A Robotic Total Station noise models

As highlighted in Section II, the uncertainty on the measurement from a RTS is
impacted by different noise sources. Each noise source can be defined with a
stochastic model, hence the possibility to use a MC method to estimate the
resulting combination of all sources of uncertainty on a single RTS
measurement. We defined a trajectory $\mathcal{P}^{i}$ in the frame
$\mathcal{F}^{i}$, where $i\in\\{1,2,3\\}$ is the index of a single RTS, as a
set of normalized homogeneous prism coordinate measurements
$\\{\bm{p}_{1}^{i},\dots,\bm{p}_{n_{i}}^{i}\\}$ such that $\bm{p}_{k}^{i}$ is
the $k^{\text{th}}$ measurement of $\mathcal{P}^{i}$ with $k\in\\{1,n_{i}\\}$
and $n_{i}\in\mathbb{N^{*}}$ is the number of measurements for the $i$-th RTS.
By merging all different kinds of noises with a MC method, we are able to
determine the spatial covariance $\bm{\Sigma}^{i}_{k}$ around each measurement
$\bm{p}_{k}^{i}$. In the following paragraphs, we define five uncertainty
models with their parameters that were used to describe the noise encountered
during our deployments with multiple RTS.

RTS instrument noises – These noise sources are directly coming from multiple
errors in the instrument calibration, namely the vertical collimation error,
the centering error, the horizontal collimation error, and the eccentricity
error. They alter the raw measurements given by the RTS, namely the distance
$\rho$, and both the horizontal and vertical angular values, $\phi$ and
$\theta$, which are used to compute prism coordinates. Their standard
deviations $\sigma_{\rho}$, $\sigma_{\phi}$ and $\sigma_{\theta}$,
respectively for the distance, horizontal and vertical deviation, are given by
manufacturers in the instrument specifications. Then, errors on measurements
can be represented by a zero-mean normal distribution, respectively
$\epsilon_{\rho}\sim\mathcal{N}(0,\sigma_{\rho})$,
$\epsilon_{\phi}\sim\mathcal{N}(0,\sigma_{\phi})$ and
$\epsilon_{\theta}\sim\mathcal{N}(0,\sigma_{\theta})$.

Tilt compensator – Modern RTS are equipped with an electronic angular
compensator that allows the instrument to correct pitch and roll values, with
its estimated gravity vector. This compensator has an inherent noise
$\epsilon_{tilt}$ represented by a zero-mean normal distribution
$\epsilon_{tilt}\sim\mathcal{N}(0,\sigma_{tilt})$ as described by [16].

Atmospheric factors and weather – Since distance measurements are taken with
EDM, they are subject to the influence of atmospheric factors, specifically
temperature $T$, pressure $P$, and humidity $H$ [8]. These atmospheric factors
are represented by uniform distributions $\epsilon_{T}$, $\epsilon_{P}$, and
$\epsilon_{H}$. According to equations proposed by [5], these uniform
distributions will lead to the estimation of a correction factor $\alpha$
(expressed in $\mathrm{p}\mathrm{p}\mathrm{m}$) to rectify a measured distance
$\rho$. The aforementioned measurement noise sources ($\epsilon_{\rho}$,
$\epsilon_{\phi}$, $\epsilon_{\theta}$, $\epsilon_{tilt}$ and the correction
$\alpha$) are combined to include uncertainties to raw RTS measurements:

$\displaystyle\widehat{\rho}$
$\displaystyle=(\rho+\epsilon_{\rho})(1+\alpha),$ ((1))
$\displaystyle\widehat{\theta}$
$\displaystyle=\theta+\epsilon_{\theta}+\epsilon_{tilt},$ ((2))
$\displaystyle\widehat{\phi}$
$\displaystyle=\phi+\epsilon_{\phi}+\epsilon_{tilt}\cot(\widehat{\theta}).$
((3))

Time synchronization – Data acquisition made by several RTS leads to a time
synchronization error $\epsilon_{t_{s}}$, expressed in seconds. The resulting
uncertainty $\bm{\epsilon_{t}}$ alters the Cartesian coordinates of a prism
and is related to the velocity $\bm{v}_{k}^{i}$ at which it moves. According
to [8], this time synchronization uncertainty follows a normal distribution
$\bm{\epsilon_{t}}\sim\mathcal{N}(\bm{\mu_{t}},\bm{\sigma_{t}})$ that depends
on the time synchronization error
$\epsilon_{t_{s}}\sim\mathcal{N}(\mu_{t_{s}},\sigma_{t_{s}})$ and the prism
velocity $\bm{v}_{k}^{i}\sim\mathcal{N}(\bm{\mu_{v}},\bm{\sigma_{v}})$:

$\displaystyle\bm{\mu_{t}}$ $\displaystyle=\mu_{t_{s}}\bm{{\mu_{v}}}$ ((4))
$\displaystyle\bm{\sigma_{t}^{2}}$
$\displaystyle=\mu_{t_{s}}^{2}\bm{\sigma_{v}^{2}}+\sigma_{t_{s}}^{2}\bm{{\mu_{v}}^{2}},$
((5))

where $\mu_{t_{s}}$ represents the mean time synchronization error,
$\sigma_{t_{s}}$ its standard deviation and
$\bm{\mu_{v}^{2}}=\begin{bmatrix}\mu_{v_{x}}^{2}&\mu_{v_{y}}^{2}&\mu_{v_{z}}^{2}\end{bmatrix}^{\intercal}$
is the square of the average prism velocity vector with a covariance
$\bm{\sigma_{v}^{2}}=\begin{bmatrix}\sigma_{v_{x}}^{2}&\sigma_{v_{y}}^{2}&\sigma_{v_{z}}^{2}\end{bmatrix}^{\intercal}$.
The prisms’ velocities are estimated by differentiating the prism Cartesian
coordinates with respect to time, by considering computed uncertainties from
eqs. (1), (2) and (3), such that:

$\displaystyle\bm{p}_{k}^{i}=\begin{bmatrix}\widehat{\rho_{k}}\sin{\widehat{\phi_{k}}}\cos{\widehat{\theta_{k}}}&\widehat{\rho_{k}}\sin{\widehat{\phi_{k}}}\sin{\widehat{\theta_{k}}}&\widehat{\rho_{k}}\cos{\widehat{\phi_{k}}}\end{bmatrix}^{\intercal}$
((6))
$\displaystyle\bm{v}_{k}^{i}=\frac{\bm{p}_{k+1}^{i}-\bm{p}_{k}^{i}}{t_{k+1}-t_{k}}.$
((7))

The values of $\bm{\mu_{v}}$ and $\bm{\sigma_{v}}$ can be estimated for each
prism position by applying a MC method to prism speeds given by eqs. (6) and
(7). A time synchronization error $\epsilon_{t_{s}}$ can be estimated over a
span of time, by taking into account the rate at which the external system’s
clock diverges from the RTS’s clock. The time synchronization method presented
by [4] yields time drift measurements, equal to the worst drift at the end of
every time synchronization period ($5\text{\,}\mathrm{min}$ in the current
case). When these measurements are recorded for all deployments on the field,
they form a distribution of drifts, among which we can statistically determine
the values of $\mu_{t_{s}}$ and $\sigma_{t_{s}}$. The estimated error
$\bm{\epsilon_{t}}$ is then added to each prism position.

Extrinsic calibration – This calibration determines the rigid transformations
${}^{W}_{i}T$ between the reference frame $\mathcal{F}^{W}$ and the frame
$\mathcal{F}^{i}$ of each RTS. Our previous work [7] exposed many extrinsic
calibration methods, including the static Ground Control Points calibration,
that will be used in this paper. As defined in [7], a GCP is a position
measured on the ground with a static prism used as a target. A number $n$ of
GCP is measured in an environment with all RTS. The outcome of this
calibration will have some noise, as the earlier-mentioned uncertainties on
single measurements propagate in the process. As the extrinsic calibration is
complex to model, its uncertainty is estimated by applying another MC method
to each GCP. The instrument noises, tilt compensator noise, and atmospheric
factors are considered for this MC method. Time synchronization was not
included due to the static nature of GCP, and because extrinsic calibration
yields results that are independent of time. An extrinsic calibration is
computed for each set of MC samples for each GCP. The resulting rigid
transformation is then applied to each prism trajectory as
$\mathcal{Q}^{i}={}^{W}_{i}\widehat{T}\;\mathcal{P}^{i}$, where
$\mathcal{Q}^{i}=\\{\bm{q}_{1}^{i},\dots,\bm{q}_{n_{i}}^{i}\\}$ represents the
prism trajectory of the $i$-th RTS in the global frame $\mathcal{F}^{W}$. The
extrinsic calibration uncertainty is estimated from the distribution of the
points along those trajectories.

Applying all the noises on RTS measurements with a MC method enables us to
estimate the covariance matrix $\bm{\Sigma}^{i}_{k}$ of each measurement
$q_{k}^{i}$ in $\mathcal{Q}^{i}$. In the rest of this paper, the frame of the
first RTS $\mathcal{F}^{1}$ is chosen as the global frame.

###  III-B Prism position uncertainty interpolation

The aim of trajectory evaluation for SLAM is to compare a reference trajectory
with a six-DOF trajectory of a robotic platform computed from various sensors
(e.g., lidar, Inertial Measurement Unit (IMU), GNSS), usually defined with
different acquisition rates. Therefore, interpolation is required to
synchronize both trajectories. A GP regression approach is chosen for this
state estimation, as proposed by [27]. This allows us to represent the prism
trajectories in continuous time in order to query position values for a
desired timestamp. To guarantee a unique solution, we modelize a prior
distribution of the potential trajectories, as a unidimensional GP, such that:

$\displaystyle\bm{x}(t)\sim\mathcal{GP}(\bm{\check{x}}(t),\bm{\check{P}}(t,t^{\prime}))\text{,}\quad
t_{0}<t,t^{\prime}$ ((8))
$\displaystyle\bm{y_{n}}=\bm{g}(\bm{x}(t_{n}))+\bm{n_{n}}\text{,}\quad
t_{1}<t_{n}<t_{N},$ ((9))

where $\bm{x}(t)$ represents the normalized homogeneous prism coordinates at
time $t$, $\bm{\check{x}}(t)$ is the prior mean function,
$\bm{\check{P}}(t,t^{\prime})$ is the prior covariance function between two
different times $t$ and $t^{\prime}$, $\bm{y_{n}}$ are measurements,
$\bm{n_{n}}\sim\mathcal{N}(\bm{0},\bm{\Sigma})$ is a Gaussian measurement
noise, $\bm{g}(\cdot)$ is a nonlinear measurement model, and
$\left\\{t_{1},\dots,t_{n},\dots,t_{N}\right\\}$ is a sequence of measurement
times.

In this paper, $\bm{y_{n}}$ are the measurements in $\mathcal{Q}^{i}$, the
covariance $\bm{\Sigma}$ of $\bm{n_{n}}$ is estimated by the MC method
presented in Section III-A (i.e., $\bm{\Sigma}^{i}_{k}$), $\bm{g}(\cdot)$ is
the non-linear process of having the measurements in $\mathcal{Q}^{i}$ by the
RTS and $t_{n}$ are the timestamps of $\bm{q}_{k}^{i}$. The interpolated
results $\widehat{\mathcal{Q}}^{i}$, which are expressed by $\bm{x}(t)$, are
computed by the STEAM library [26] for desired query times. As a result, each
estimated point $\widehat{\bm{q}}^{i}_{j}$ in $\widehat{\mathcal{Q}}^{i}$ has
its associated estimated covariance matrix $\widehat{\bm{\Sigma}}^{i}_{j}$
coming from the GP interpolation, where $j\in\\{1,J\\}$ is the interpolated
prism positions index, and $J\in\mathbb{N^{*}}$ is the total number of
interpolated prism positions.

###  III-C Uncertainty propagation to ground truth trajectory

With only one RTS, the uncertainty $\widehat{\bm{\Sigma}}^{i}$ on the
reference trajectory can be exploited right away to evaluate the reference
position of a robotic platform. However, the robotic platform’s pose needs to
be evaluated in six-DOF. With three RTS, it is possible to obtain the
reference pose by doing a point-to-point minimization between the triplets of
measured prism coordinates, and the reference triplets measured in laboratory
[4, 7]. Prism uncertainties can be propagated by applying a MC sampling with
this point-to-point minimization.

After the GP interpolation, a setup of three RTS yields a set of three paths
$\\{\widehat{\mathcal{Q}}^{1},\widehat{\mathcal{Q}}^{2},\widehat{\mathcal{Q}}^{3}\\}$
of interpolated measurements, with their respective covariance
$\\{\widehat{\bm{\Sigma}}^{1},\widehat{\bm{\Sigma}}^{2},\widehat{\bm{\Sigma}}^{3}\\}$.
We define $\widehat{\mathcal{Q}}_{j}$ as the $j$-th triplet of interpolated
prism positions with its corresponding triplet $\widehat{\bm{\Sigma}}_{j}$ of
covariances, such that
$\widehat{\mathcal{Q}}_{j}=\\{\widehat{\bm{q}}^{1}_{j},\widehat{\bm{q}}^{2}_{j},\widehat{\bm{q}}^{3}_{j}\\}$,
and
$\widehat{\bm{\Sigma}}_{j}=\\{\widehat{\bm{\Sigma}}^{1}_{j},\widehat{\bm{\Sigma}}^{2}_{j},\widehat{\bm{\Sigma}}^{3}_{j}\\}$,
as shown in Figure 2. A reference triplet $\mathcal{R}$ contains normalized
homogeneous points $\bm{r}_{i}$, where
$\mathcal{R}=\\{\bm{r}_{1},\bm{r}_{2},\bm{r}_{3}\\}$ with a covariance
$\bm{U}_{i}$ associated to each point $\bm{r}_{i}$. These reference points are
defined in another world frame $\mathcal{F}^{L}$ and were statically estimated
with measurements from a single RTS after each deployment. To apply a MC
method, we sample $M$ points
$\left\\{s_{1}^{i},\dots,s_{m}^{i},\dots,s_{M}^{i}\right\\}$ from every
Gaussian distribution
$\mathcal{N}(\widehat{\bm{q}}^{i}_{j},\widehat{\bm{\Sigma}}^{i}_{j})$, and $M$
points from the Gaussian distribution $\mathcal{N}(\bm{r}_{i},\bm{U}_{i})$
defined for the triplet of points in $\mathcal{R}$ along with their
covariances $\bm{U}_{i}$.

For every sample $s_{m}\in\\{s_{1},\dots,s_{M}\\}$, we applied the point-to-
point minimization with:

${}^{1}_{L}\widehat{\bm{T}}_{j,m}=\operatorname*{arg\,min}_{\bm{T}}\sum_{i=1}^{3}\mathinner{\\!\left\lVert\widehat{\bm{q}}^{i}_{j}-\bm{T}{\bm{r}}_{i}\right\rVert}^{2}_{2},$
((10))

where ${}^{1}_{L}\widehat{\bm{T}}_{j,m}\in\mathbb{R}^{4\times 4}$ is the
resulting rigid transformation of the MC method between the frame
$\mathcal{F}^{L}$ and the global frame of prism measurements
($\mathcal{F}^{1}$, in the current case), for a sample $s_{m}$ in the $j$-th
triplet.

Figure 2: Visualization of error propagation of the Monte Carlo (MC) method
applied with a point-to-point minimization. Each triplet of interpolated prism
measurements $\widehat{\bm{q}}^{i}_{j}$ of trajectories
$\widehat{\mathcal{Q}^{i}}$ in the world frame $\mathcal{F}^{W}$ are denoted
by crosses enclosed within ellipses representing their corresponding
covariances $\widehat{\bm{\Sigma}}^{i}_{j}$. The points $\bm{r}_{1}$,
$\bm{r}_{2}$, and $\bm{r}_{3}$ are the reference prism positions taken in
laboratories, along with their covariance. A point-to-point minimization
minimizes the distance between samples of the same color, in both kinds of
distributions. The result of this minimization is the estimated vehicle pose
$\bm{\xi}_{j}$ along with its covariance $\bm{\Lambda}_{j}$.

The subsequent $M$ poses form a distribution, of which we can extract an
average translation and rotation defined by $\bm{\xi}_{j}\in\mathbb{R}^{6}$ in
the global frame $\mathcal{F}^{W}$ for every $j\in\\{1,J\\}$. The covariance
$\bm{\Lambda}_{j}\in\mathbb{R}^{6\times 6}$ of this distribution of poses
yields the uncertainty on every vehicle pose $\bm{\xi}_{j}$. The resulting
reference trajectory is defined as the set $\xi$ of poses
$\left\\{\bm{\xi}_{1},\dots,\bm{\xi}_{j},\dots,\bm{\xi}_{J}\right\\}$.
$\Lambda$ is a set that contains the covariance $\bm{\Lambda}_{j}$ of every
pose $\bm{\xi}_{j}$ along $\xi$. These ground truth uncertainties can be used
for the evaluation of six-DOF trajectories. In the next sections, we will
characterize the impact of the source of noise over the uncertainty models of
the prism trajectories and of the reference trajectory.

##  IV Experiments

We used three Trimble S7 RTS to track three Trimble MultiTrack Active Target
MT1000 prisms with a measurement rate of $2.5\text{\,}\mathrm{Hz}$. The Table
I gives the different kinds of noises that were modeled, in accordance with
the specifications of the Trimble S7. Following the GUM guidelines [19], these
noises have been divided into two types (i.e., A and B). The former is
determined through experimental values (e.g., extrinsic calibration, time
synchronization error). The latter is given by the specifications of the
measuring instrument (e.g., range, angle, tilt compensator), or from an
environmental model (i.e., atmospheric factors).

TABLE I: Trimble S7 RTS uncertainty parameters. These uncertainties have been categorized according to their type, source, distribution and values. Zero means are considered for each normal distribution, except for the time synchronization error as described in Section III-A. All standard deviation values are given for $2\sigma$ according to the ISO norm ISO17123-3. The abbreviation ppm means parts per million, and $$1\text{\,}\mathrm{"}$=$4.85\text{\times}{10}^{-6}\text{\,}\mathrm{rad}$$ _Influence factors_ | _Distribution_ | _Values_
---|---|---
| Extrinsic calibration |  |
| \- Translation | Normal |  $\sigma_{tx}$, $\sigma_{ty}$, $\sigma_{tz}$
Type A | \- Rotation | Normal |  $\sigma_{rx}$, $\sigma_{ry}$, $\sigma_{rz}$
| Time synchronization |  |
| \- Velocity | Normal |  $\mu_{v}$, $\sigma_{v}$
| \- Time error | Normal |  $\mu_{t_{s}}=$1.2\text{\,}\mathrm{ms}$$, $\sigma_{t_{s}}=$0.8\text{\,}\mathrm{ms}$$
| Instrument |  |
| \- Distances | Normal | $\sigma_{\rho}=$4\text{\,}\mathrm{m}\mathrm{m}$+$2\text{\,}\mathrm{p}\mathrm{p}\mathrm{m}$$
| \- Horizontal directions | Normal | $\sigma_{\phi}=$2\text{\,}\mathrm{"}$$
| \- Vertical directions | Normal | $\sigma_{\theta}=$2\text{\,}\mathrm{"}$$
Type B | Tilt compensator |  |
| \- Angle bias | Normal | $\sigma_{tilt}=$0.5\text{\,}\mathrm{"}$$
| Atmospheric factors |  |
| \- Temperature | Uniform | $\sigma_{T}=[0,$1\text{\,}\mathrm{\SIUnitSymbolCelsius}$]$
| \- Pressure | Uniform | $\sigma_{P}=[0,$10\text{\,}\mathrm{hPa}$]$
| \- Humidity | Uniform | $\sigma_{H}=[0,$2\text{\,}\mathrm{\char 37\relax}$]$

As shown in Figure 1, all three prisms were mounted on a Clearpath Warthog
Unmanned Ground Vehicle (UGV). A Robosense RS-32 and a XSens MTi-10 IMU were
used as part of an Iterative Closest Point (ICP)-based SLAM framework, working
at a rate of $10\text{\,}\mathrm{Hz}$.222https://github.com/norlab-
ulaval/norlab_icp_mapper The experiments were conducted from February 2022 to
January 2023. They include $20$ deployments, of which $18$ took place on the
campus of Université Laval and two were done in the Montmorency research
forest, $75\text{\,}\mathrm{km}$ north of Quebec City. These 20 deployments
allowed us to conduct 48 experiments, for a total of $50\text{\,}\mathrm{km}$
of RTS-tracked prism trajectories.

The same procedure was applied during each experiment, in order to collect
consistent and standardized data during the whole year. Also, each deployment
was completed by measuring accurately the position of the three prisms,
rigidly installed on the robot, with a single RTS. These measurements are used
as reference points to compute the inter-prism distances, as a way to control
data for each experiment. The point-to-point minimization method presented in
Section III-C is also relying on these measurements. Weather conditions and
atmospheric values were obtained through the weather service of Environment
and Climate Change
Canada.333https://climate.weather.gc.ca/historical_data/search_historic_data_e.html

##  V Results

###  V-A Influence of the sources of uncertainty over the results

We first evaluated the impact of different sources of noise on the prism
position uncertainty. These sources of noise are 1) the RTS instrument noises,
2) the tilt compensator noises, 3) the atmospheric factors, 4) the time
synchronization, and 5) the extrinsic calibration. Every source of noise was
represented by a distinct covariance matrix, on which we applied the Frobenius
norm [25] to evaluate their effect on the prism position uncertainty. These
uncertainties were also compared for different ranges, to determine how it is
impacted by the RTS-prism distance.

Figure 3 shows that, for every noise source except the time synchronization
noise, a longer range will lead to higher uncertainty. This relation is
especially the case for extrinsic calibration, with a median value of
$0.91\text{\,}\mathrm{mm}$ in the range of $0$ to $75\text{\,}\mathrm{m}$, a
median value of $1.88\text{\,}\mathrm{mm}$ in the range of $75$ to
$150\text{\,}\mathrm{m}$, and a median value of $5.49\text{\,}\mathrm{mm}$ in
the range of more than $150\text{\,}\mathrm{m}$. Moreover, this noise source
affects the majority of the total uncertainty for the complete range, with a
median value of $1.32\text{\,}\mathrm{mm}$. This observation is coherent with
the description of extrinsic calibration given in Section III-A, as this
calibration relies on measurements that are all impacted by the other sources
of uncertainty, causing its covariance to be higher. However, this impact on
the uncertainty is only considerable at a long-range, while the noises
inherent to the RTS have the highest median regardless of the distance. With a
median value of $2.1\text{\,}\mathrm{mm}$, we confirmed that the uncertainty
level from the instrument is in the range of the manufacturer’s
specifications. This noise also increases with long-range measurements, with a
median value of $2.78\text{\,}\mathrm{mm}$ for distances of more than
$150\text{\,}\mathrm{m}$. Meanwhile, the other sources of noise were less
significant. The time synchronization noise has a median value of
$0.4\text{\,}\mathrm{mm}$, and does not depend on the measurement range.
Similarly, the atmospheric factors have a median value of
$0.16\text{\,}\mathrm{mm}$, while the tilt noise has a median value of
$0.06\text{\,}\mathrm{mm}$. Both of these noise sources increase according to
the measurement range.

Figure 3: Influence of the noise sources from Table I, in relation with the
measured distance between an RTS and its assigned prism. The square root of
the Frobenius norm is used to estimate the similarity between covariance
matrices.

In a field deployment, it would be important to keep in mind the two factors
that have the highest influence on the results. Therefore, it is crucial to
achieve a good extrinsic calibration, as it is the main source of uncertainty
for long-range measurements. Otherwise, it is important to gather as much data
as possible with ranges lower than $150\text{\,}\mathrm{m}$. The median for
all sources on the complete range is close to the median for shorter ranges,
as we gathered more data at short distances than at long distances:
$80\text{\,}\%$ of the data was taken with distances between $0$ and
$75\text{\,}\mathrm{m}$, $14\text{\,}\%$ between $75$ and
$150\text{\,}\mathrm{m}$ and $6\text{\,}\mathrm{\char 37\relax}$ for more than
$150\text{\,}\mathrm{m}$. Consequently, the results could be impaired by the
lack of long-range measurements. Overall, since the RTS has an inherent noise,
better results could be obtained with other instruments that would be more
precise.

###  V-B Trajectories with uncertainty

We used the pipeline from [7] to filter the raw prism measurements to increase
the accuracy of the results. The modules ($1$ and $2$) from this pipeline were
used with the parameters $\tau_{r}=$2\text{\,}\mathrm{m}\,\mathrm{s}^{-1}$$,
$\tau_{a}=\tau_{e}=$1\text{\,}\mathrm{d}\mathrm{e}\mathrm{g}\,\mathrm{s}^{-1}$$,
$\tau_{s}=$3\text{\,}\mathrm{s}$$ and $\tau_{l}=$2\text{\,}\mathrm{s}$$.
Instead of using linear interpolation in the third module, we computed a GP
interpolation with the STEAM library. This GP was used to interpolate the
uncertainties from the MC method, as explained in Section III-B.

An example of this interpolation is shown in Figure 4, which represents the
results of a deployment at the Montmorency forest. The interpolated prism
measurements are displayed with red, blue, and green dots, along with their
uncertainties as shaded ellipsoids. The orange dots represent measurements
from a GNSS system on the robot that took data at a rate of
$5\text{\,}\mathrm{Hz}$. As in the fourth module of the pipeline in [7], the
uncertainty has been filtered for values over $20\text{\,}\mathrm{cm}$, while
the inter-prism distances are kept under $10\text{\,}\mathrm{cm}$ to ensure
that the values are precise enough for ground truth generation. The point-to-
point method described in Section III-C propagates the prisms uncertainties
among the reference pose of the Warthog, as shown with black dots in Figure 4.
The six-DOF pose and uncertainties on the ground truth trajectory can be
compared with an estimated robot trajectory through the use of other metrics
than the Euclidean norm.

Figure 4: Top view of a reference trajectory generated from a deployment in
the Montmorency forest. Interpolated prism paths $\widehat{\mathcal{Q}}^{i}$
are represented by dots in red (prism 1), blue (prism 2), and green (prism 3).
The RTS-estimated pose $\bm{\xi}$ is displayed by black points, along with a
GNSS-estimated position represented in orange. Both covariances
$\widehat{\bm{\Sigma}}^{i}_{j}$ and $\bm{\Lambda}_{j}$ are shown with shaded
ellipsoids. RTS positions are indicated by an indigo cross, along with the
start and stop positions indicated by red stars.

Even if RTS measurements are more accurate than GNSS
($2$-$3\text{\,}\mathrm{cm}$), they gather fewer data over time. Therefore,
with the GP interpolation, the uncertainty on the RTS measurements increases
over time. It can reach as much as $5\text{\,}\mathrm{cm}$, as shown in the
zoomed section of Figure 4. This issue can be solved by using RTS with a
higher measurement rate. Moreover, the MC method used with the point-to-point
method spreads the error on the final robot pose. Finally, as RTS requires
direct line-of-sight with a prism, fewer data can be measured in obstructed
environments such as forests. This constraint is visible on Figure 4, where
the RTS-estimated poses only appear in areas with a direct line-of-sight from
the RTS.

###  V-C Impact of models over pose-uncertainty results

The Figure 5 shows that the uncertainty on the position and orientation of a
robot is not prominently affected by a single source of noise. For instance,
no matter the source of uncertainty, the medians are $2.5\text{\,}\mathrm{m}$
and $0.76\text{\,}\mathrm{rad}$ for the position and the orientation,
respectively. This stability might come from the point-to-point minimization
which smoothens the trajectory and therefore minimizes some of the errors that
could be caused by the different sources of uncertainty (i.e., uncertainty
inherent to the RTS, the tilt, the atmospheric conditions, the extrinsic
calibration, and the time synchronization).

Figure 5: Impact of noise sources on the final pose uncertainty, for both the
translation and the orientation, respectively in blue and red.

The values given by the Frobenius norm square root for the robot position (in
Figure 5) are larger by an order of magnitude than the uncertainty computed on
a prism position (in Figure 3). This can be related to the GP interpolation in
the pipeline, as the interpolation drastically increases the uncertainty in
proportion to the speed of the vehicle. Also, the point-to-point minimization
propagates the prism uncertainties on the vehicle pose uncertainty. These
results can be compared to the one obtained by [4], where they show the same
kind of uncertainty on the final pose of a vehicle, with a comparable amount
of uncertainty on the prism positions.

##  VI Conclusion

In this paper, we proposed a MC method to model the uncertainties coming from
multiple RTS with the intent to better compare six-DOF trajectories. The
estimated uncertainty of a prism measurement is then interpolated with a GP,
and propagated to the estimated six-DOF pose of a robotic platform with a MC
method, used with a point-to-point minimization. We have highlighted that the
main source of noise when using multiple RTS is coming from the extrinsic
calibration, besides the uncertainty that is inherent to the instrument. Our
model has demonstrated that the uncertainty on a prism measurement is
proportional to the distance between that prism and a RTS. Moreover, none of
the sources of noise have a certain impact on the uncertainty of a pose that
is computed with point-to-point minimization. This can be caused by the
minimization method that smoothens the effect of different noise sources to an
average value.

Future works would include the optimization of our extrinsic calibration
method to minimize resulting uncertainties. Other atmospheric factors such as
snow or rain would also need to be experimentally characterized. The
uncertainty of GNSSs could be modeled in the same manner to compare it with
the uncertainty obtained with our method. This would allow us to evaluate
localization and mapping algorithms by merging information from RTS-based
ground truth trajectories with GNSS-based ground-truth trajectories.

## Acknowledgment

This research was supported by the Natural Sciences and Engineering Research
Council of Canada (NSERC) through the grant CRDPJ 527642-18 SNOW (Self-driving
Navigation Optimized for Winter).

## References

  * [1] Lintong Zhang et al.  “Hilti-Oxford Dataset: A Millimeter-Accurate Benchmark for Simultaneous Localization and Mapping”  In _IEEE Robotics and Automation Letters_ 8.1 Institute of ElectricalElectronics Engineers (IEEE), 2023, pp. 408–415  DOI: 10.1109/lra.2022.3226077
  * [2] Ursula Kälin, Louis Staffa, David Eugen Grimm and Axel Wendt  “Highly Accurate Pose Estimation as a Reference for Autonomous Vehicles in Near-Range Scenarios”  In _Remote Sensing_ 14.1, 2022  DOI: 10.3390/rs14010090
  * [3] François Pomerleau, Ming Liu, Francis Colas and Roland Siegwart  “Challenging data sets for point cloud registration algorithms”  In _The International Journal of Robotics Research_ 31.14, 2012, pp. 1705–1711  DOI: 10.1177/0278364912458814
  * [4] Maxime Vaidis, Philippe Giguere, Francois Pomerleau and Vladimir Kubelka  “Accurate outdoor ground truth based on total stations”  In _2021 18th Conference on Robots and Vision (CRV)_ IEEE, 2021  DOI: 10.1109/crv52889.2021.00012
  * [5] J.. Rueger and University New South Wales.  “Refractive indices of light, infrared and radio waves in the atmosphere / Jean M. Rueger”  School of SurveyingSpatial Information Systems, University of New South Wales Sydney, 2002, pp. vi92 p.
  * [6] François Pomerleau  “Robotics in Snow and Ice”  In _Encyclopedia of Robotics_ Berlin, Heidelberg: Springer Berlin Heidelberg, 2023, pp. 1–6
  * [7] Maxime Vaidis et al.  “Extrinsic calibration for highly accurate trajectories reconstruction”  arXiv, arXiv preprint arXiv:2210.01048, 2022  DOI: 10.48550/ARXIV.2210.01048
  * [8] Thomas Ulrich  “Uncertainty estimation and multi sensor fusion for kinematic laser tracker measurements”  In _Metrologia_ 50.4 IOP Publishing, 2013, pp. 307  DOI: 10.1088/0026-1394/50/4/307
  * [9] Daniel Loret de Mola Lemus, David Kohanbash, Scott Moreland and David Wettergreen  “Slope Descent using Plowing to Minimize Slip for Planetary Rovers”  In _Journal of Field Robotics_ 31.5, 2014, pp. 803–819  DOI: 10.1002/rob.21518
  * [10] Vladimir Kubelka et al.  “Robust Data Fusion of Multimodal Sensory Information for Mobile Robots”  In _Journal of Field Robotics_ 32.4, 2015, pp. 447–473  DOI: 10.1002/robb.21535
  * [11] Gregory Hitz, François Pomerleau, Francis Colas and Roland Siegwart  “Relaxing the planar assumption: 3D state estimation for an autonomous surface vessel”  In _The International Journal of Robotics Research_ 34.13 SAGE Publications, 2015, pp. 1604–1621  DOI: 10.1177/0278364915583680
  * [12] Kirk MacTavish, Michael Paton and Timothy D. Barfoot  “Selective memory: Recalling relevant experience for long-term visual localization”  In _Journal of Field Robotics_ 35.8, 2018, pp. 1265–1292  DOI: 10.1002/rob.21838
  * [13] Patrik Schmuck and Margarita Chli  “CCM-SLAM: Robust and efficient centralized collaborative monocular simultaneous localization and mapping for robotic teams”  In _Journal of Field Robotics_ 36.4, 2019, pp. 763–781  DOI: 10.1002/rob.21854
  * [14] Eva Reitbauer, Christoph Schmied and Manfred Wieser  “Autonomous navigation module for tracked compost turners”  In _2020 European Navigation Conference, ENC 2020_ , 2020, pp. 1–10  DOI: 10.23919/ENC48637.2020.9317465
  * [15] Felipe A.C. Rodriguez, Luis A.K. Veiga and Wilson A. Soares  “Temperature Acquisition System for Real Time Application of First Velocity Correction by EDM (Electronic Distance Measurement)”  In _Geoplanning_ 8.1, 2021, pp. 61–74  DOI: 10.14710/geoplanning.8.1.61-74
  * [16] Werner Lienhart, Matthias Ehrhart and Magdalena Grick  In _Journal of Applied Geodesy_ 11.1, 2017, pp. 1–8  DOI: doi:10.1515/jag-2016-0028
  * [17] Tomas Thalmann and Hans Neuner  “Temporal calibration and synchronization of robotic total stations for kinematic multi-sensor-systems”  In _Journal of Applied Geodesy_ 15.1, 2021, pp. 13–30  DOI: 10.1515/jag-2019-0070
  * [18] Edward Morse and Victoria Welty  “Dynamic testing of laser trackers”  In _CIRP Annals_ 64.1, 2015, pp. 475–478  DOI: https://doi.org/10.1016/j.cirp.2015.04.090
  * [19] BIPM et al.  “Evaluation of measurement data — Guide to the expression of uncertainty in measurement”, Joint Committee for Guides in Metrology, JCGM 100:2008  URL: https://www.bipm.org/documents/20126/2071204/JCGM_100_2008_E.pdf/cb0ef43f-baa5-11cf-3f85-4dcd86f77bd6
  * [20] Lauryna Šiaudinytė and Kenneth Thomas Victor Grattan  “Uncertainty evaluation of trigonometric method for vertical angle calibration of the total station instrument”  In _Measurement_ 86, 2016, pp. 276–282  DOI: https://doi.org/10.1016/j.measurement.2015.10.037
  * [21] Fumin Zhang, Xinghua Qu, Hongyan Wu and Shenghua Ye  “Evaluation of uncertainty in large-scale fusion metrology”  In _Fourth International Symposium on Precision Mechanical Measurements_ 7130 SPIE, 2008, pp. 71305P  International Society for OpticsPhotonics  DOI: 10.1117/12.819765
  * [22] Scott Ferson  “What Monte Carlo methods cannot do”  In _Human and Ecological Risk Assessment: An International Journal_ 2.4 Informa UK Limited, 1996, pp. 990–1007  DOI: 10.1080/10807039609383659
  * [23] Gaoge Hu, Bingbing Gao, Yongmin Zhong and Chengfan Gu  “Unscented kalman filter with process noise covariance estimation for vehicular ins/gps integration system”  In _Information Fusion_ 64 Elsevier BV, 2020, pp. 194–204  DOI: 10.1016/j.inffus.2020.08.005
  * [24] Dailys Arronde Pérez, Harald Gietler and Hubert Zangl  “Automatic Uncertainty Propagation Based on the Unscented Transform”  In _2020 IEEE International Instrumentation and Measurement Technology Conference (I2MTC)_ , 2020, pp. 1–6  DOI: 10.1109/I2MTC43012.2020.9129581
  * [25] Timothy D. Barfoot and Paul T. Furgale  “Associating Uncertainty With Three-Dimensional Poses for Use in Estimation Problems”  In _IEEE Transactions on Robotics_ 30.3 Institute of ElectricalElectronics Engineers (IEEE), 2014, pp. 679–693  DOI: 10.1109/TRO.2014.2298059
  * [26] Sean Anderson and Timothy D. Barfoot  “Full STEAM ahead: Exactly sparse gaussian process regression for batch continuous-time trajectory estimation on SE(3)”  In _2015 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ IEEE, 2015, pp. 157–164  DOI: 10.1109/IROS.2015.7353368
  * [27] Sean Anderson, Timothy D. Barfoot, Chi Hay Tong and Simo Särkkä  “Batch Nonlinear Continuous-Time Trajectory Estimation as Exactly Sparse Gaussian Process Regression”  In _Auton. Robots_ 39.3 USA: Springer ScienceBusiness Media LLC, 2015, pp. 221–238  DOI: 10.1007/s10514-015-9455-y

  *[SLAM]: Simultaneous Localization and Mapping
  *[GNSS]: Global Navigation Satellite System
  *[RTS]: Robotic Total Station
  *[DOF]: Degrees Of Freedom
  *[MC]: Monte Carlo
  *[EDM]: Electronic optical Distance Measurement
  *[GNSSs]: Global Navigation Satellite System
  *[GP]: Gaussian Process
  *[DOFs]: Degrees Of Freedom
  *[UAV]: Unmanned Aerial Vehicle
  *[GUM]: Guide to the expression of Uncertainty in Measurement
  *[STEAM]: Simultaneous Trajectory Estimation And Mapping
  *[GCP]: Ground Control Point
  *[IMU]: Inertial Measurement Unit
  *[UGV]: Unmanned Ground Vehicle
  *[ICP]: Iterative Closest Point