# GNSS/Multi-Sensor Fusion Using Continuous-Time Factor Graph Optimization for
Robust Localization

Haoming Zhang1, , Chih-Chun Chen1,
Heike Vallery1,2, and Timothy D. Barfoot3 1H. Zhang, C.-C. Chen, and H.
Vallery are with the Institute of Automatic Control, Faculty of Mechanical
Engineering, RWTH Aachen University, Aachen, Germany.2H. Vallery is also with
the Department of BioMechanical Engineering, Delft University of Technology,
and with the Department for Rehabilitation Medicine, Erasmus MC, Rotterdam,
The Netherlands.3T. D. Barfoot is with the University of Toronto Robotics
Institute, Toronto, Canada.Corresponding author<EMAIL_ADDRESS>
###### Abstract

Accurate and robust vehicle localization in highly urbanized areas is
challenging. Sensors are often corrupted in those complicated and large-scale
environments. This paper introduces GNSS-FGO, an online and global trajectory
estimator that fuses GNSS observations alongside multiple sensor measurements
for robust vehicle localization. In GNSS-FGO, we fuse asynchronous sensor
measurements into the graph with a continuous-time trajectory representation
using Gaussian process regression. This enables querying states at arbitrary
timestamps so that sensor observations are fused without requiring strict
state and measurement synchronization. Thus, the proposed method presents a
generalized factor graph for multi-sensor fusion. To evaluate and study
different GNSS fusion strategies, we fuse GNSS measurements in loose and tight
coupling with a speed sensor, IMU, and lidar-odometry. We employed datasets
from measurement campaigns in Aachen, Düsseldorf, and Cologne in experimental
studies and presented comprehensive discussions on sensor observations,
smoother types, and hyperparameter tuning. Our results show that the proposed
approach enables robust trajectory estimation in dense urban areas, where the
classic multi-sensor fusion method fails due to sensor degradation. In a test
sequence containing a $17\text{\,}\mathrm{km}$ route through Aachen, the
proposed method results in a mean 2D positioning error of
$0.19\text{\,}\mathrm{m}$ for loosely coupled GNSS fusion and
$0.48\text{\,}\mathrm{m}$ while fusing raw GNSS observations with lidar
odometry in tight coupling.

###### Index Terms:

GNSS, Factor Graph Optimization, Localization, Sensor Fusion, Autonomous
Vehicle Navigation

##  I Introduction

Safe and reliable autonomous driving operations in urban areas require
accurate and consistent vehicle localization that infers a smooth trajectory
estimate for planning and control tasks. Autonomous vehicles may use global
navigation satellite systems (GNSS) to achieve global positioning in large-
scale environments. However, the performance of GNSS is highly degraded when a
vehicle passes through tunnels or urban canyons, where GNSS signal loss can be
expected, greatly penalizing positioning availability. Moreover, the error
dynamics of GNSS observations grow increasingly complex due to multipath and
non-line-of-sight effects, resulting in inconsistent error models used in
state estimation [1].

(a) Lidar map and trajectory using LIO-SAM[2]. This approach fused with GNSS
positioning failed due to faulty scan registrations while crossing a tunnel
and degrades dramatically with corrupted GNSS measurements.

(b) Lidar map and trajectory using GNSS-FGO (ours). The proposed method
provides robust trajectory estimation and suffers from only slight degradation
in GNSS-corrupted areas.

(c) Estimated trajectories. The fisheye images show challenging areas where
the GNSS observations are blocked or strongly corrupted.

Figure 1: Demonstration of multi-sensor fusion for vehicle localization of
test sequence C02 in the city of Cologne.

Many previous works fuse information from local optical sensors (e.g., lidars
or cameras) for vehicle localization. They can typically be categorized into
pose retrieval using a given map [3] and simultaneous location and mapping
(SLAM) [4]. Generally, landmarks in sensor frames are extracted and associated
to acquire either frame-to-map global pose constraints or frame-to-frame local
motion increments. Lacking high-quality maps for vehicle pose retrieval in
many areas, approaches relying on local sensors can often only achieve
satisfactory localization if the ground is even and sufficient loop-closure
constraints help eliminate drift. However, these requirements cannot always be
met for long-term autonomous operations in large-scale environments [5].

In recent years, combining local sensors with GNSS has been investigated as a
robust way to enable accurate and precise vehicle location in challenging
areas. Incremental batch estimation implemented as factor graph optimization
(FGO) is often superior to classic filtering-based algorithms in terms of
localization performance and consistency [6, 7]. Unlike Bayesian filters, a
factor graph fuses prior information and sensor measurements associated with
the to-be-estimated state variables into probabilistic representations. A
maximum-a-posterior problem (MAP) can be formulated from the factor graph and
solved in a batch configuration using iterative Gauss-Newton-like algorithms
[8]. In general, this optimization procedure is activated only if new sensor
observations are available. Thus, many classic graph FGO approaches rely on a
primary sensor that schedules the optimization procedure.

To fuse additional sensor modalities, asynchronous measurements must be
synchronized with the primary sensor, leading to information loss and
inefficient fusion mechanisms. Furthermore, classic FGO approaches degrade if
the primary sensor is compromised or fails, which is likely in challenging
environments. In this case, state variables cannot be effectively constrained
by other sensor observations if the graph is not constructed in time. Fig. 1
exemplifies this problem, where a state-of-the-art lidar-centric SLAM approach
diverges due to scan registration failures while driving in a tunnel111Same
factors, noise models, and smoother were used while benchmarking the lidar-
centric approach with loosely coupled GNSS-FGO.. In fact, as discussed in [9,
10, 11], commonly used sensors in localization deteriorate under challenging
environmental conditions, complicating robust and long-term vehicle
localization.

In this work, we address the degradation problem of GNSS-based localization
approaches by translating classic FGO for multi-sensor fusion into an approach
where the graph associated with all to-be-estimated state variables is
constructed deterministically based on a priori chosen timestamps. It thus
presents a time-centric factor graph construction that is independent of any
particular reference sensor (e.g., GNSS). To achieve this, we represent the
vehicle trajectory in continuous time using a Gaussian process (GP). This
approach incorporates a motion prior using the white-noise-on-jerk (WNOJ)
motion model, as originally proposed in [12]. The algorithm feeds new
observations from each sensor independently into the factor graph without
measurement-to-state synchronization. If a measurement cannot be temporally
aligned with any state variable, we query a GP-interpolated state
corresponding to the measurement that is used for the error evaluation within
the optimization procedures.

To retrieve a robust global trajectory estimation while the GNSS measurements
are strongly corrupted, we implemented the time-centric factor graph to fuse
GNSS observations with measurements of an inertial measurement unit (IMU),
optical speed sensor, and lidar for vehicle localization in challenging urban
scenarios. We propose two factor graph structures for both loosely and tightly
coupled fusion of GNSS observations alongside other local sensor measurements,
demonstrating the flexibility of the proposed GNSS-FGO. For the graph that
considers the GNSS positioning solution in the loose coupling, we fuse the
pre-integrated IMU measurements, 2D velocity measurements, and lidar odometry.
In tightly coupled fusion, we replace GNSS solution factors with GNSS
pseudorange and deltarange factors, which are expected to provide more
effective constraints compared to inconsistent GNSS positioning in urban areas
[6].

We used raw data from measurement campaigns in the cities of Aachen,
Düsseldorf, and Cologne to evaluate the proposed approach by benchmarking with
a well-known lidar-centric SLAM approach [2, 13]. This lidar-centric SLAM has
been shown to perform best for vehicle localization tasks in large-scale
environments and can be configured to fuse GNSS measurements [14], which
presents an equivalent fusion mechanism as our loosely coupled GNSS-FGO.

In contrast to our previous study [7], which focused only on trajectory
smoothness using FGO with an offline evaluation, we now address online multi-
sensor fusion for vehicle localization.

The contributions of this work are summarized as follows:

  1. 1.

We propose a flexible, online, continuous-time factor graph optimization
framework that can accommodate common multi-sensor fusion problems. The
flexibility comes from the fact that (i) we can accommodate asynchronous
measurements, and (ii) we choose estimation timestamps independent of any
particular sensor frequency. This latter feature, as well as the smoothing
effect of a motion prior, provide robustness in the phase of any particular
sensor dropout.

  2. 2.

We implement the proposed method for vehicle localization in challenging
scenarios and conduct comprehensive studies on loosely coupled and tightly
coupled fusion mechanisms to fuse GNSS measurements with other local sensors,
with the aim of presenting extensive evaluations and discussions on accuracy,
robustness, and run-time efficiency. Compared to other state-of-the-art
methods, our method is shown to be more robust in our challenging test
scenarios.

  3. 3.

We evaluate the GP motion prior, which is implemented using the white-noise-
on-acceleration (WNOA) and white-noise-on-jerk (WNOJ) models, to study the
accuracy of the interpolated states.

  4. 4.

We introduce a scalable and open-source software framework
gnssFGO222https://github.com/rwth-irt/gnssFGO that can be extended for
arbitrary robot localization using continuous-time factor graph optimization.

The rest of this article is organized as follows: we present a comprehensive
review of the literature on multi-sensor fusion in the context of vehicle
location using FGO in Sec. II. Sec. III introduces the proposed continuous-
time FGO in detail. In Sec. IV, the mathematical background for factor
formulations is presented, whereas the graph implementations for vehicle
localization are introduced in Sec. V. We verify our method in Sec. VII and
conduct further experiments and ablation studies on the precision and
consistency of estimated trajectories in different scenarios. Finally, Sec.
VIII summarizes results and limitations. We release our code and raw data used
in our experiments from real-world measurement campaigns with different urban
scenarios222https://github.com/rwth-irt/gnssFGO. A demonstration video is also
available333https://youtu.be/9R55uCCrNss.

##  II Related Work

###  II-A Graph Optimization for GNSS-based Vehicle Localization

In recent years, fusing GNSS observations using factor graph optimization for
robust vehicle localization has drawn great attention. Compared with
filtering-based approaches, FGO conducts batch optimization, where all
measurement models are re-linearized and re-evaluated iteratively, resulting
in a more robust state estimation even with measurement outliers. Previous
work demonstrated robust localization in urban areas only by factoring
pseudoranges with robust error models [15, 16]. Later, Wen et al.,[6] and
Zhang et al.,[7] showed that FGO generally outperforms Kalman filters with
respect to the precision and smoothness of the estimated trajectory.

GNSS data can be integrated into the graph using a loosely or tightly coupled
schema [17]. While the loosely coupled fusion incorporates GNSS positioning
solution into the graph, pre-processed raw GNSS observations such as code or
carrier phase measurements can be fed into the estimator in a tight coupling
as state constraints. As the to-be-estimated state variables can be directly
observed in GNSS solutions, fusing GNSS data in a loose coupling enables quick
convergence and elevated accuracy if high-quality Real-Time-Kinematic
(RTK)-fixed GNSS solutions are available. In contrast, the integration of raw
GNSS observations contributes to multiple state constraints associated with
received satellites, which has been shown to be more robust than loose
coupling [18, 6, 19]. Wen et al. [20] included double-differenced pseudorange
(DDPR) and double-differenced carrier-phase measurements (DDCP) in FGO,
resulting in performance improvement. Later, this work was extended to
efficiently model carrier-phase constraints between multiple satellite
measurement epochs within a time window [21]. In [22], time-differenced
carrier-phase (TDCP) was integrated with cycle-slip estimation, which achieved
accurate localization while presenting substantial availability compared to
DDCP if satellites can be continuously tracked. Congram and Barfoot [23] also
proposed a GPS odometry using TDCP with a more prominent cycle-slip detection
and showed an effective drift reduction compared to visual odometry. However,
since carrier-phase observations are also disturbed in deep urban areas, the
robustness of state estimation cannot yet be guaranteed.

As factor graph optimization presents a convenient tool for robust error
modeling [24], several works employ m-Estimators to reject faulty GNSS
observations [16, 25, 22, 26, 7]. Recently, FGO has been explored in the
context of vehicle location based on GNSS for noise distribution
identification or adaptive rejection of outliers [27, 28], showing a positive
impact on consistent trajectory estimation using FGO.

###  II-B Graph Optimization for Multi-Sensor Fusion

While the aforementioned works have particularly explored graph optimization
for GNSS observations, they may still suffer from performance degeneration in
complex scenarios if GNSS measurements are lost or present outliers.
Therefore, another research domain focuses on fusing more sensor modalities
(more than two) alongside GNSS observations into the graph, with applications
predominantly in SLAM.

A pose graph that fuses GPS position measurements and lidar odometry with
loop-closure constraints for outdoor scenarios improved both runtime
efficiency and performance compared to lidar-only approaches [29]. In [2],
feature-based lidar odometry and loop-closure constraints were merged into a
factor graph with synchronized GPS position measurements to achieve a drift-
free pose estimate, which was forwarded to another graph optimization with
pre-integrated IMU measurements for high-frequency and real-time state
estimation. In addition to integrating feature-based lidar odometry into FGO,
the lidar map can also be used for GNSS visibility assessment [30]. Some works
also introduce camera-centric sensor fusion, where other sensor observations
are synchronized with camera data and fused on the graph [31, 32, 33]. In
[34], camera, lidar, and wheel odometers were fused into the graph along with
the GNSS positioning solution and IMU measurements, presenting consistent
localization in featureless environments for long-term runs. Similar works
also conduct multi-sensor fusion without GNSS and propose a carefully managed
fusion architecture [35, 36]. However, these works still require well-handled
data synchronization and careful graph construction to fuse heterogeneous
sensor measurements.

Many recent approaches introduce multi-graph structures to achieve flexible
and compact sensor fusion. In [37], IMU, GNSS, and lidar observations were
separately integrated into multiple graphs in parallel with a switching
mechanism. When the GNSS receiver lost its signal, the lidar-centric graph was
activated. Another work aimed to confederate loosely and tightly coupled
fusion schemes to ensure the estimation performance [38]. Each sensor modality
is associated with a separate graph and proposes odometry factors to the IMU-
centric graph that provides final estimated states in real-time.

Because the aforementioned methods introduce redundant and complex graph
structures, other works exploit high-frequency IMU measurements to coordinate
multi-sensor fusion. In [39] and [40], asynchronous global pose measurements
(e.g., GPS measurements) are propagated into timestamps of visual-inertial
factors using pre-integrated IMU measurements. The same concept has been
extended to a forward-backward IMU pre-integration mechanism in order to
precisely associate asynchronous measurements with keyframes [41].
Nevertheless, these methods still depend on the noisy IMU sensor, which
introduces uncertainty.

###  II-C Continuous-Time Trajectory Representation

One essential requirement for flexible graph-based multi-sensor fusion is the
ability to query the states associated with the observations within the
iterative optimization process. This requirement can be fulfilled if the
trajectory is represented in continuous time. In [42], B-splines were proposed
as a parametric approach to represent the trajectory in continuous time. This
method was later used to propose stereo-inertial odometry [43]. Another
approach utilizes exactly sparse Gaussian process (GP) regression by assuming
that system dynamics follow a linear time-varying stochastic differential
equation (LTV-SDE) [44]. The system dynamics are typically modeled as white-
noise-on-acceleration (WNOA). This approach was verified in [45, 46, 47],
where the reliability of this proposed surrogate dynamics model was
demonstrated. Recently, Tang et al. [12] proposed an improved system dynamics
model, which assumed a white-noise-on-jerk (WNOJ) model in LTV-SDE. They
showed that the WNOJ could model the vehicle dynamics more accurately and
thus, was appropriate for systems with more complicated dynamics. This work
follows this aspect and adapts the GP-WNOJ model proposed in [12] as between-
state motion constraints and state interpolator to fuse asynchronous
measurements.

Although continuous-time trajectory representation is studied for localization
and mapping problems by extending incremental smoothing using sparse GP
interpolation to reduce computation time [46], fusing GNSS observations with
multiple heterogeneous sensor measurements for online vehicle localization has
not yet been presented or discussed. Inspired by the aforementioned methods,
we address the problem of multi-sensor fusion for GNSS-based vehicle
localization using continuous-time trajectory representation, which enables a
fusion of asynchronous sensor observations in a single factor graph. Our
hypotheses are: (i) factor graph construction in continuous time generalizes
multi-sensor fusion and enables consistent trajectory estimation that
incorporates effective state constraints from multiple sensor modalities in
challenging scenarios, (ii) the GP-WNOJ motion model presents a larger
capacity to represent complicated system dynamics, such as driving in urban
areas.

##  III Time-Centric Factor Graph Optimization

In this section, we introduce an implementation of continuous-time trajectory
estimation, as proposed in [44, 12]. Generally, fusing multiple heterogeneous
sensor observations into a state estimator incorporates different timestamps
due to asynchronous measurements and unpredictable delays. In this work, we
assume that the state estimator and all measurements have the same timing
clock. Compared to the estimated states in continuous time, all sensor
observations are sampled and processed in asynchronous timestamps, as
illustrated in Fig. 2. We employ GP motion priors that enable a continuous-
time trajectory representation. In this way, the construction of a factor
graph can be deterministic and time-centric, bypassing asynchronous sensor
frequencies and timing issues. We show the general structure of a time-centric
factor graph in Fig. 3, where the to-be-estimated state variables
${\bm{x}}_{t}$ are presented in solid line circles on a continuous-time
trajectory. The queried states in dashed lines are not explicitly estimated in
the optimization procedure, and thus are only queried between two successive
state variables using the time offset $\tau$ to the previous state variable.

Figure 2: Continuous-time state estimation with asynchronous measurements. A
time offset $\tau$ can be calculated with respect to a former state variable
for each asynchronous measurement. Figure 3: A general time-centric factor
graph. The state variables ${\bm{x}}_{t}$ are created and constrained with GP
motion prior factors on time while all asynchronous measurements are fused by
querying a state with a time offset $\tau$ to a former state variable.

Input : Last state id and timestamp pair
$({x}_{\mathrm{id}}^{-},\leavevmode\nobreak\ {x}_{\mathrm{ts}}^{-})$

Propagated states
${\bm{x}}_{k}^{-}\in\bm{\mathcal{X}^{-}},\leavevmode\nobreak\ k=1...n$

List of sensor measurements ${s}\in\bm{\mathcal{S}}$

Output : Current state id and timestamp pair
$({x}_{\mathrm{id}}^{+},\leavevmode\nobreak\ {x}_{\mathrm{ts}}^{+})$

Optimized state ${\bm{x}}_{k}^{+}$ and uncertainties ${\bm{P}}_{k}^{+}$

1
$\bm{\mathcal{G}}\leftarrow\textnormal{{initGraph}}({\bm{x}}_{0}^{-},\leavevmode\nobreak\
{\bm{P}}_{0}^{-})$;

2 List of state id and timestamp pairs $\bm{\mathcal{P}}=\emptyset$;

3 ${x}_{\mathrm{id}}={x}_{\mathrm{id}}^{-}$;

4 for  _$k=1:n$_ do

5 ${x}_{\mathrm{id}}^{+}={x}_{\mathrm{id}}+1$;

6
${x}_{\mathrm{ts}}^{+}=\textnormal{{updateTimestamp}}({x}_{\mathrm{ts}}^{-})$;

7
$\bm{\mathcal{G}}\leftarrow\textnormal{{NewStateVariable}}({x}_{\mathrm{id}},\leavevmode\nobreak\
{x}_{\mathrm{ts}},\leavevmode\nobreak\ \bm{\mathcal{X}})$;

8
$\bm{\mathcal{G}}\leftarrow\textnormal{{GPMotionFactor}}({x}_{\mathrm{id}-1},\leavevmode\nobreak\
{x}_{\mathrm{id}})$;

9 $\bm{\mathcal{P}}\leftarrow({x}_{\mathrm{id}},\leavevmode\nobreak\
{x}_{\mathrm{ts}})$;

10

11 end for

12for  _Each sensor ${s}\in\bm{\mathcal{S}}$_ do

13 for  _Each observation $k=1:m$_ do

14 $({x}_{i}^{\mathrm{id}},\leavevmode\nobreak\
{x}_{i}^{\mathrm{ts}},\leavevmode\nobreak\ \tau,\leavevmode\nobreak\ type)$
$\leftarrow\textnormal{{queryStateInfo}}({\mathrm{timestamp}}_{k}^{{\bm{s}}},\leavevmode\nobreak\
\bm{\mathcal{P}})$;

15 if  _$type$ is DROPPED_ then

16 /* Measurements in the past. */;

17 $\textnormal{{discardMeasurement}}({\bm{o}}_{k}^{s})$;

18

19 else if  _$type$ is SYNCHRONIZED_ then

20
$\bm{\mathcal{G}}\leftarrow\textnormal{{SensorFactor}}({x}_{\mathrm{id}},\leavevmode\nobreak\
{\bm{o}}_{k}^{s})$;

21

22 else if  _$type$ is INTERPOLATED_ then

23
$\bm{\mathcal{G}}\leftarrow\textnormal{{GPSensorFactor}}({x}_{i}^{\mathrm{id}},\leavevmode\nobreak\
{x}_{i+1}^{\mathrm{id}},\leavevmode\nobreak\ \tau,\leavevmode\nobreak\
{\bm{o}}_{k}^{{\bm{s}}})$;

24

25 else if  _$type$ is CACHED_ then

26 /* Measurements in the future. */
$\textnormal{{cacheMeasurement}}({\bm{o}}_{k}^{s})$;

27

28

29 end for

30

31 end for

32

33$\\{{\bm{x}}_{k}^{+},\leavevmode\nobreak\ {\bm{P}}_{k}^{+}\\}\leftarrow$
$\hskip
10.00002pt\textnormal{{doOptimizationAndMarginalization}}(\bm{\mathcal{G}})$;

return _$\\{({x}_{\mathrm{id}}^{+},\leavevmode\nobreak\
{x}_{\mathrm{ts}}^{+}),\leavevmode\nobreak\
{\bm{x}}_{k}^{+},\leavevmode\nobreak\ {\bm{P}}_{k}^{+}\\}$_

Algorithm 1 Time-centric factor graph optimization

Alg. 1 explains one optimization procedure from graph construction to
iterative optimization. Assume that the time-centric factor graph is extended
with $n$ new to-be-estimated state variables in each procedure. We extend the
graph with $n$ state variables and create GP motion prior factors that
constrain the relative state transitions between two successive state
variables. In doing so, the timestamps of all state variables are chosen
deterministically. While solving the iterative optimization problem, an
initial prediction ${\bm{x}}_{k}^{-}\in\bm{\mathcal{X}^{-}}$ must be provided
for each state variable. These predictions can be acquired using prior motion
models (e.g., GP state extrapolation [48]). In this work, we utilize state
propagation using IMU measurements to calculate the initial estimate of future
states at high frequency.

As new sensor observations are received at different timestamps in parallel to
state estimation, we retrieve the cached $m$ observations from each sensor
$s\in\mathcal{S}$ in a second loop. We define a time threshold
${t}_{\mathrm{sync}}$ for state-observation alignment to query the index of
related state variables. If state variables can be associated with sensor
observations within this threshold, normal sensor factors are added to the
graph. Otherwise, we construct the measurement factors by querying a GP
interpolated state aligned with the measurement timestamp. In this case, two
successive state variables ${\bm{x}}_{i}$ and
${\bm{x}}_{j},\leavevmode\nobreak\ j=i+1$ are obtained with a time offset
$\tau$ between the measurement and the former state ${\bm{x}}_{i}$.

After graph construction, we employ a Gauss-Newton-like optimizer to solve the
MAP problem [49]. The optimized state ${\bm{x}}_{k}^{+}$ and marginalized
uncertainties ${\bm{P}}_{k}^{+}$ are returned for further state propagation,
as introduced in Sec. V-E.

##  IV Mathematical Background

###  IV-A Frames

In this article, we consider the following frames for state representation and
illustrate them in Fig. 4:

  1. 1.

World Geodetic System (WGS84) frame: The WGS84 frame presents the vehicle’s
coordinates (latitude $\varphi$, longitude $\lambda$, and height $h$) in a
unified geodetic system [50]. In this work, we transform the position into the
WGS84 frame for visualization and do not consider projected coordinate systems
to avoid coordinate distortions.

  2. 2.

Earth-Centered, Earth-Fixed (ECEF) frame: The ECEF frame, denoted as
$(\cdot)^{e}$, formulates a Cartesian coordinate system at the center of
Earth’s mass and can be transformed into the WGS84 frame in closed form. We
define the pose and velocity of our system in the ECEF frame for the sake of
convenience.

  3. 3.

Navigation frame: As the vehicle is moving in a local tangent plane on Earth,
the north-east-down (NED) frame and the east-north-up (ENU) frame are commonly
used as navigation frames $(\cdot)^{n}$ to present the vehicle’s velocity and
orientation in planning and control tasks [17]. In this work, vehicle velocity
and rotation are transformed into the frame $(\cdot)^{n}$ for error metrics in
experimental studies.

  4. 4.

Local World Frame: As shown in [2, 32], odometry information acquired by local
sensors (e.g., camera or lidars) refers to an arbitrary local tangent frame
that is determined on system initialization with respect to an initial vehicle
pose in the navigation frame. We denote this frame as a local world frame
$(\cdot)^{w}$.

  5. 5.

Body Frame: We denote the body frame aligned with the IMU sensor center as
$(\cdot)^{b}$. The body frame also represents the local pose ${\bm{\eta}}(t)$
of the GP motion models.

Figure 4: Coordinate frames used in this work.

###  IV-B Notation

To present the state variables in different frames, we use ${\bm{R}}_{b}^{e}$
and ${\bm{t}}_{b}^{e}$ to denote the rotational and translational parts from
frame $b$ to frame $e$. This notation is extended as ${\bm{R}}_{b,t}^{e}$ to
represent the states with respect to time $t$. For motion increments in the
same frame, we simplify the notation as $\Delta{\bm{t}}_{ij}$ to represent the
translational offset of two timestamps $i$ and $j$. We follow the pose
representation
${\bm{T}}_{b}^{e}=\Big{[}\begin{smallmatrix}{\bm{R}}_{b}^{e}&{\bm{t}}_{b}^{e}\\\
{\bm{0}}&1\end{smallmatrix}\Big{]}\in SE(3)$ to calculate the motion increment
[51]. For high-dimensional transition matrices in GP motion models (e.g.,
${\bm{\Lambda}}(\tau)\in\mathbb{R}^{18\times 18}$), we denote the subblocks
${\bm{\Lambda}}_{mn}\in\mathbb{R}^{6\times 6}$ associated with different state
components for linear state querying in (16).

###  IV-C Continuous-Time Trajectory Representation using GP

Barfoot et al. [44] originally proposed a continuous-time trajectory
representation using Gaussian process regression, which presents an exactly
sparse kernel by assuming the system dynamics follow a linear time-varying
stochastic differential equation (LTV-SDE):

$\displaystyle\begin{split}\dot{{\bm{\gamma}}}(t)&={\bm{A}}{\bm{\gamma}}(t)+{\bm{B}}{\bm{u}}(t)+{\bm{F}}{\bm{w}}(t),\\\
{\bm{w}}(t)&\sim\mathcal{GP}({\bm{0}},\leavevmode\nobreak\
{\bm{Q}}_{c}\cdot\delta(t-t^{{}^{\prime}})),\end{split}$ (1)

where the vector ${\bm{\gamma}}(t)$ represents a local state variable. The
time-varying system matrices are denoted as ${\bm{A}},\leavevmode\nobreak\
{\bm{B}}$ and ${\bm{F}}$, respectively. The input vector ${\bm{u}}(t)$ is set
to ${\bm{0}}$. The process noise ${\bm{w}}(t)$ is given as a zero-mean
Gaussian process (GP) with the kernel function formulated with the power
spectral density matrix ${\bm{Q}}_{c}\in\mathbb{R}^{6\times 6}$ and the Dirac
delta function, $\delta$.

In discrete time, this state-space model can be furthermore interpreted to
interpolate an arbitrary state at timestamp $t_{\tau}$ (
$t_{i}<t_{\tau}<t_{j}$) given an initial local state ${\bm{\gamma}}_{i}$ using

$\displaystyle{\bm{\gamma}}_{i}(t_{\tau})={\bm{\Lambda}}(t_{\tau}){\bm{\gamma}}_{i}(t_{i})+{\bm{\Omega}}(t_{\tau}){\bm{\gamma}}_{i}(t_{j}),$
(2)

where

$\displaystyle{\bm{\Lambda}}(t_{\tau})$
$\displaystyle={\bm{\Phi}}(t_{\tau},t_{i})-{\bm{\Omega}}(t_{\tau}){\bm{\Phi}}(t_{\tau},t_{j}),$
(3) $\displaystyle{\bm{\Omega}}(t_{\tau})$
$\displaystyle={\bm{Q}}_{i,t_{\tau}}{\bm{\Phi}}(t_{\tau},t_{j})^{T}{\bm{Q}}_{i,j}^{-1}.$
(4)

The system transition matrix ${\bm{\Phi}}$ in (3) and (4) can be defined using
a white-noise-on-acceleration (WNOA, a.k.a., constant-velocity) prior, as
demonstrated in earlier works [44, 52]. Later, Tang et al. [12] introduced a
white-noise-on-jerk (WNOJ) prior that presents third-order system dynamics
with the system transition function

$\displaystyle{\bm{\Phi}}(t,t_{i})=\left[\begin{matrix}{\bm{1}}&(t_{i}-t){\bm{1}}&\frac{1}{2}(t_{i}-t)^{2}{\bm{1}}\\\
{\bm{0}}&{\bm{1}}&(t_{i}-t){\bm{1}}\\\
{\bm{0}}&{\bm{0}}&{\bm{1}}\end{matrix}\right].$ (5)

The time-varying covariance matrix ${\bm{Q}}_{i}(t)\in\mathbb{R}^{18\times
18}$ and its precision matrix ${\bm{Q}}_{i}^{-1}(t)$ are computed as

$\displaystyle{\bm{Q}}_{i}(t)=\left[\begin{matrix}\frac{1}{20}\Delta
t_{i}^{5}{\bm{Q}}_{c}&\frac{1}{8}\Delta
t_{i}^{4}{\bm{Q}}_{c}&\frac{1}{6}\Delta t_{i}^{3}{\bm{Q}}_{c}\\\
\frac{1}{8}\Delta t_{i}^{4}{\bm{Q}}_{c}&\frac{1}{3}\Delta
t_{i}^{3}{\bm{Q}}_{c}&\frac{1}{2}\Delta t_{i}^{2}{\bm{Q}}_{c}\\\
\frac{1}{6}\Delta t_{i}^{3}{\bm{Q}}_{c}&\frac{1}{2}\Delta
t_{i}^{2}{\bm{Q}}_{c}&\Delta t_{i}{\bm{Q}}_{c}\end{matrix}\right],$ (6)
$\displaystyle{\bm{Q}}_{i}^{-1}(t)=\left[\begin{matrix}720\Delta
t_{i}^{-5}{\bm{Q}}_{c}^{-1}&-360\Delta t_{i}^{-4}{\bm{Q}}_{c}^{-1}&60\Delta
t_{i}^{-3}{\bm{Q}}_{c}^{-1}\\\\[2.0pt] -360\Delta
t_{i}^{-4}{\bm{Q}}_{c}^{-1}&192\Delta t_{i}^{-3}{\bm{Q}}_{c}^{-1}&-36\Delta
t_{i}^{-2}{\bm{Q}}_{c}^{-1}\\\\[2.0pt] 60\Delta
t_{i}^{-3}{\bm{Q}}_{c}^{-1}&-36\Delta t_{i}^{-2}{\bm{Q}}_{c}^{-1}&9\Delta
t_{i}^{-1}{\bm{Q}}_{c}^{-1}\end{matrix}\right].$ (7)

Compared to other approaches, trajectory representation (interpolation) using
Gaussian process regression effectively incorporates physics-driven models to
retrieve realistic vehicle motion by scaling the transition function with the
time-varying covariance matrix ${\bm{Q}}$. As the hyper-parameter
${\bm{Q}}_{c}$ can be tuned for different applications [53], this approach can
be extended for nonlinear problems (see Sec. IV-D) and enables more accurate
state interpolation [7, 47].

###  IV-D GP-WNOJ Motion Prior Model

Following the approach in [12], a GP motion prior for $SE(3)$ can be defined
as

$\displaystyle\begin{split}\dot{{\bm{T}}}(t)&={\bm{\varpi}}(t)^{\land}{\bm{T}}(t),\\\
\dot{{\bm{\varpi}}}(t)&={\bm{w}}(t),\end{split}$ (8)

where the vehicle pose in the global frame is denoted as ${\bm{T}}(t)$, which
can be calculated as ${\bm{T}}(t)=\exp{({\bm{\xi}}(t)^{\land})}$ with local
pose ${\bm{\xi}}(t)=[{\bm{\rho}}(t)^{T}\leavevmode\nobreak\
{\bm{\phi}}(t)^{T}]^{T}\in\mathbb{R}^{6}$. The vectors ${\bm{\rho}}(t)$ and
${\bm{\phi}}(t)$ represent the position and orientation of a local pose (e.g.,
in the body frame) [54]. A local pose can be converted to $\mathfrak{se}(3)$
by applying the operator $(\cdot)^{\land}$. The operator $(\cdot)^{\lor}$ is
the inverse of $(\cdot)^{\land}$ [51]. The vector
${\bm{\varpi}}(t)=[{\bm{\nu}}(t)^{T}\leavevmode\nobreak\
{\bm{\omega}}(t)^{T}]^{T}\in\mathbb{R}^{6}$ represents the body-centric
velocity. With this motion prior, the state of the GP motion model in a global
frame is given as

$\displaystyle{\bm{x}}(t)=\\{{\bm{T}}(t),\leavevmode\nobreak\
{\bm{\varpi}}(t),\leavevmode\nobreak\ \dot{{\bm{\varpi}}}(t)\\}\in
SE(3)\times\mathbb{R}^{12}.$ (9)

However, the GP motion prior in (8) cannot be implemented directly using (1)
due to nonlinearity of the system dynamics. To address this problem, Anderson
and Barfoot [52] showed that a local linear GP prior can be defined between
each state-timestamp pair, $t_{i}$ and $t_{i+1}$, by transforming the global
pose ${\bm{T}}(t)$ into the local tangent frame, where a local pose
${\bm{\xi}}(t)$ can be calculated as

$\displaystyle{\bm{\xi}}_{i}(t)=\ln({\bm{T}}(t){\bm{T}}_{t_{i}}^{-1})^{\lor},\leavevmode\nobreak\
\leavevmode\nobreak\ t_{i}\leq t\leq t_{i+1},$ (10)

where we consider the pose ${\bm{T}}_{t_{i}}$ at the timestamp $t_{i}$ as a
fixed parameter while formulating the local pose ${\bm{\xi}}_{i}(t)$ for an
arbitrary pose ${\bm{T}}(t)$ for $t>t_{i}$.

Because the motion between state-timestamp pairs, which are usually associated
with high-frequent measurement timestamps (e.g., lidar at
$10\text{\,}\mathrm{Hz}$), is generally small, this local GP prior
approximately represents a linear, time-invariant (LTI) SDE, which can be
driven from (1) by assuming the system matrices remain constant. Thus, a local
state variable of GP-WNOJ prior for $SE(3)$ can be defined as

$\displaystyle{\bm{\gamma}}(t)=[{\bm{\xi}}(t)^{T}\leavevmode\nobreak\
\dot{{\bm{\xi}}}(t)^{T}\leavevmode\nobreak\ \ddot{{\bm{\xi}}}(t)^{T}]^{T}$
(11)

and propagated using (2) to (4). The time derivatives of the local pose can be
calculated as

$\displaystyle\dot{{\bm{\xi}}}(t)$
$\displaystyle=\bm{\mathcal{J}}({\bm{\xi}}_{i}(t))^{-1}{\bm{\varpi}}(t),$ (12)
$\displaystyle\ddot{{\bm{\xi}}}(t)$
$\displaystyle=-\frac{1}{2}(\bm{\mathcal{J}}({\bm{\xi}}(t))^{-1}{\bm{\varpi}}(t))^{\curlywedge}{\bm{\varpi}}(t)+\bm{\mathcal{J}}({\bm{\xi}}(t))^{-1}\dot{{\bm{\varpi}}}(t),$
(13)

where the matrix $\bm{\mathcal{J}}$ is the left Jacobian of $SE(3)$ [51]. To
calculate $\frac{d(\bm{\mathcal{J}}^{-1})}{dt}$ in closed form for (13), we
approximately formulate
$\bm{\mathcal{J}}^{-1}\approx{\bm{1}}-\frac{1}{2}{\bm{\xi}}^{\curlywedge}$
[12]. The operator $({\bm{\xi}})^{\curlywedge}$ represents the adjoint of
${\bm{\xi}}^{\wedge}\in\mathfrak{se}(3)$ [51], which can be calculated as

$\displaystyle{\bm{\xi}}^{\curlywedge}=\left[\begin{matrix}{\bm{\rho}}\\\
{\bm{\phi}}\end{matrix}\right]=\left[\begin{matrix}{\bm{\phi}}^{\wedge}&{\bm{\rho}}^{\wedge}\\\
{\bm{0}}&{\bm{\phi}}^{\wedge}\end{matrix}\right].$ (14)

Because the left Jacobian requires several matrix calculations, it can be
approximated as an identity matrix ${\bm{1}}$ over small intervals to improve
the computation efficiency444We implement this trick as a configuration in the
proposed GNSS-FGO. [55].

Given a local state variable that represents the origin system state for each
state-timestamp pair, we can retrieve the WNOJ motion model for two successive
local state variables in the local frame as

$\displaystyle\begin{split}{\bm{\gamma}}_{i}(t_{i})&=[{\bm{0}}\leavevmode\nobreak\
{\bm{\varpi}}(t)^{T}\leavevmode\nobreak\ \dot{{\bm{\varpi}}}(t)^{T}]^{T},\\\
{\bm{\gamma}}_{i}(t_{i+1})&=\left[\begin{matrix}\ln({\bm{T}}_{i+i,i})^{\lor}\\\
\bm{\mathcal{J}}_{i+1}^{-1}{\bm{\varpi}}_{i+1}\\\
-\frac{1}{2}(\bm{\mathcal{J}}_{i+1}^{-1}{\bm{\varpi}}_{i+1})^{\curlywedge}{\bm{\varpi}}_{i+1}+\bm{\mathcal{J}}_{i+1}^{-1}\dot{{\bm{\varpi}}}_{i+1}\end{matrix}\right].\end{split}$
(15)

Using the GP-WNOJ prior, a state at an arbitrary time $\tau\in(i,i+1)$ can be
queried as

$\displaystyle\begin{split}{\bm{T}}_{\tau}&=\exp\Big{\\{}[{\bm{\Lambda}}_{12}(\tau){\bm{\varpi}}_{i}+{\bm{\Omega}}_{13}(\tau)\dot{{\bm{\varpi}}}_{i}+{\bm{\Sigma}}_{11}(\tau)\ln({\bm{T}}_{i+i,i})^{\lor}+\\\
&+{\bm{\Omega}}_{12}(\tau)\bm{\mathcal{J}}_{i+1}^{-1}{\bm{\varpi}}_{i+1}+\\\
&+{\bm{\Omega}}_{13}(\tau)(-\frac{1}{2}(\bm{\mathcal{J}}_{i+1}^{-1}{\bm{\varpi}}_{i+1})^{\curlywedge}{\bm{\varpi}}_{i+1}+\bm{\mathcal{J}}_{i+1}^{-1}\dot{{\bm{\varpi}}}_{i+1})]^{\land}\Big{\\}}{\bm{T}}_{i},\end{split}$
(16)

where ${\bm{\Lambda}}$ and ${\bm{\Omega}}$ are vehicle transition matrices
obtained from (2) to (6).

######  Remark 1.

As discussed in [12], representing a realistic system transition using the GP
motion priors requires proper tuning of the power spectral density matrix
${\bm{Q}}_{c}$. In this work, we assume that ${\bm{Q}}_{c}$ is a constant
diagonal matrix defined as ${\bm{Q}}_{c}=\mathrm{diag}({\bm{q}}_{c})$ with a
6D hyper-parameter ${\bm{q}}_{c}$. For a fair evaluation of the GP-WNOA and
GP-WNOJ models, we do not tune this hyper-parameter explicitly in each test
sequence. We formulate ${\bm{Q}}$ in both GP models with the same
parameterization for pose weighting.

###  IV-E Measurement Models

####  IV-E1 GNSS Observations

Generally, a single antenna GNSS receiver can provide both position, velocity,
and time (PVT) solutions and raw observations. As the pose and velocity of the
PVT solution can be directly associated with the state variables in the FGO,
we only present the measurement models for the raw GNSS observations:
pseudorange $\rho$ and Doppler shift (a.k.a., deltarange) $\nu$.

In localization approaches that tightly fuse the GNSS observations,
pseudorange and deltarange are commonly used and well studied [17]. The
pseudorange $\rho$ represents a geometric distance between the phase center of
the GNSS antenna and the associated satellite, which is calibrated in a pre-
processing step to eliminate satellite orbit bias and atmospheric delays, as
shown in [56, 7]. After pre-processing, the pseudorange can be calculated with
respect to the antenna position as

$\tilde{{\rho}}_{k}=\left\|{\bm{t}}_{a}^{e}-{\bm{t}}_{\mathrm{sat},k}^{e}\right\|_{2}+{c}_{b}+\delta\rho_{M}+w_{\rho},$
(17)

where the vector ${\bm{t}}_{a}^{e}$ and ${\bm{t}}_{\mathrm{sat},k}^{e}$
represent the positions of GNSS antenna and $k$-th satellite in ECEF frame,
respectively. The variable ${c}_{b}$ represents the receiver clock bias for
the corresponding satellite source. The measurement noise is denoted as
$w_{\rho}$. The multi-path error $\delta\rho_{M}$ is neglected in this work.
We filter out all GNSS observations from satellites with an elevation angle
less than $15\text{\,}\mathrm{\SIUnitSymbolDegree}$555This is an ad-hoc
choice, generally used in ground vehicle navigation..

The deltarange $\nu$, or Doppler velocity, is measured as the internal carrier
frequency change of the GNSS receiver while moving relative to the
corresponding satellite. With this observation, the vehicle velocity
concerning the satellite velocity can be represented as

$\tilde{{\nu}}_{k}=({\bm{e}}_{\mathrm{sat},k}^{e})^{T}({\bm{v}}_{\mathrm{sat},k}^{e}-{\bm{v}}_{a}^{e})+{c}_{d}+w_{\nu,k}.$
(18)

In (18), the unit vector ${\bm{e}}_{\mathrm{sat},k}^{e}$ represents the
direction from the antenna to the $k$-th satellite. We denote the satellite
and antenna velocities in the ECEF frame as ${\bm{v}}_{\mathrm{sat},k}^{e}$
and ${\bm{v}}_{a}^{e}$, respectively. The receiver clock drift and measurement
noise are given as ${c}_{d}$ and $w_{\nu,k}$.

####  IV-E2 Lidar Odometry

We adapt the feature extraction and matching methods from a feature-based
lidar odometry [13, 2] to obtain the relative motion increments between two
laser keyframes. The coordinates of raw lidar points acquired in different
timestamps are re-calibrated using the IMU measurement to the original
timestamp of the lidar scan. We classify the calibrated points into edge and
planar features, ${\bm{F}}_{t}=\\{{\bm{F}}_{t}^{e},\leavevmode\nobreak\
{\bm{F}}_{t}^{p}\\}$, based on the smoothness metric shown in [13, 57]. In
scan registration, all $k$ features in ${\bm{F}}_{t+1}$ of the current scan
are associated with pose priors ${\bm{T}}_{t+1,1:k}^{w}$ and used to find the
best transformation $\Delta{\bm{T}}_{t,t+1}^{w}$ from the last laser scan by
solving an optimization problem that takes the distance between the
corresponding features in ${\bm{F}}_{t}$ using a Gauss-Newton algorithm.

In [2], a lidar-centric SLAM approach is presented that optionally fuses GPS
positioning. This approach can only present accurate state estimates if the
scan registration converges and sufficient global references (e.g., GPS
position or loop closure) are available. In contrast to [2], we query the
vehicle states at scan timestamps from a previously built time-centric graph
and integrate the transformation $\Delta\tilde{{\bm{T}}}_{t,t+1}^{w}$ as
between-pose constraints. After the graph optimization, we query the optimized
states again using the GP motion model and update lidar keyframe poses in
frame ${w}$ using the following transformation

$\displaystyle{\bm{T}}_{l,t}^{w}={\bm{T}}_{l,\rm
anc}^{e,-1}{\bm{T}}_{l,t}^{e},$ (19)

where the transformation matrices ${\bm{T}}_{l,t}^{e}$ and
${\bm{T}}_{l,t}^{w}$ denote lidar poses in frame ${e}$ and frame ${w}$,
respectively. As lidar odometry requires a state-space representation in a
local-world (a.k.a., local-tangent) frame ${w}$ where the $z$-axis is gravity
aligned, we query an anchor pose ${\bm{T}}_{l,\rm
anc}^{e}=\Big{[}\begin{smallmatrix}{\bm{R}}_{l,\rm anc}^{e}&{\bm{t}}_{l,\rm
anc}^{e}\\\ {\bm{0}}&1\end{smallmatrix}\Big{]}$ of the lidar sensor on first
scan and initialize a local-world frame of the lidar odometry by setting the
anchor pose as its origin. In contrast to [32], a coarse orientation
estimation is unnecessary in our work to align the local-world frame and the
navigation frame because the vehicle orientation is given.

####  IV-E3 Optical Speed Sensor

We employ a high-grade vehicle optical speed sensor that provides unbiased 2D
velocity observations $\bar{{\bm{v}}}_{t}^{b}$ in the body frame at
$100\text{\,}\mathrm{Hz}$. The 2D velocity observations can be associated with
the vehicle velocity in the state vector using

$\displaystyle\tilde{{\bm{v}}}_{t}^{b}=\left[\begin{matrix}\tilde{{v}}_{t,x}^{b}\\\
\tilde{{v}}_{t,y}^{b}\end{matrix}\right]=\left[\begin{matrix}1&0&0\\\ 0&1&0\\\
\end{matrix}\right]\cdot{\bm{R}}_{e}^{b}{\bm{v}}_{b}^{e},$ (20)

where the vector $\tilde{{\bm{v}}}_{t}^{b}$ represents the observed 2D
velocity components in frame ${b}$, which can be evaluated with the vehicle
velocity variable ${\bm{v}}_{b}^{e}$ transformed with the inverse rotation
matrix ${\bm{R}}_{e}^{b}$ back to frame ${b}$.

##  V FGO for Vehicle Localization

This section presents our implementation of the proposed GNSS-FGO for two
sensor fusion schemes. In loosely coupled fusion, we obtain a baseline
trajectory of our datasets by fusing the GNSS-PVT solution with the observed
2D vehicle velocity from a high-grade speed sensor and the lidar odometry. To
defend the superiority of fusing raw GNSS observations for vehicle
localization, we propose a tightly coupled fusion of raw GNSS observations
with IMU measurements and lidar odometry, which is evaluated with the baseline
trajectory. In this section, we introduce all probabilistic factor
formulations and the proposed factor graph structures.

###  V-A State Variables

The state variable at timestamp $t$ in this work is defined as

$\displaystyle{\bm{x}}_{t}\triangleq\\{{\bm{T}}_{b,t}^{e}\leavevmode\nobreak\
{\bm{v}}_{b,t}^{e}\leavevmode\nobreak\
{\bm{b}}_{b,t}^{\mathrm{a}}\leavevmode\nobreak\
{\bm{b}}_{b,t}^{\mathrm{g}}\leavevmode\nobreak\ {\bm{c}}_{t}^{\mathrm{r}}\\}.$
(21)

We estimate the vehicle pose ${\bm{T}}_{b,t}^{e}\in SE(3)$ and 6D velocity
${\bm{v}}_{b}^{e}$ in frame ${e}$. The vectors ${\bm{b}}_{b,t}^{\rm a}$ and
${\bm{b}}_{b,t}^{\rm g}$ denote the 3D biases of the accelerometer and
gyroscope, respectively. The 2D vector ${\bm{c}}_{t}^{\rm
r}=[{c}_{b,t}\leavevmode\nobreak\ {c}_{d,t}]^{T}$ represents the GNSS receiver
clock bias ${c}_{b,t}$ and drift ${c}_{d,t}$, which is only estimated by the
tightly coupled fusion of raw GNSS observations.

######  Remark 2.

Unlike [12], we do not estimate 6D accelerations in GP motion models to reduce
the dimension of the state vector. Instead, we consider the vehicle
accelerations measured by the IMU as inputs to the WNOJ model.

###  V-B Factor Formulations

####  V-B1 Pre-Integrated IMU Factor

In graph-optimization-based state estimation approaches, the IMU pre-
integration, introduced in [58, 59], is generally utilized to integrate high-
frequency IMU measurements as between-state factors for the optimization
procedures running at a lower rate. The pre-integrated IMU measurements
represent the relative motion increments on manifold. These relative motion
increments can be assumed unchanged while re-linearizing the consecutive state
variables in the optimization iterations, resulting in efficient computation.
Following [59], we define the error function of the IMU factor between two
consecutive state variables at timestamps $t_{i},\leavevmode\nobreak\ t_{j}$
as

$\displaystyle\left\|{\bm{e}}_{ij}^{\mathrm{imu}}\right\|^{2}=\left\|[{\bm{r}}_{\Delta{\bm{R}}_{ij}}^{T}\leavevmode\nobreak\
{\bm{r}}_{\Delta{\bm{v}}_{ij}}^{T}\leavevmode\nobreak\
{\bm{r}}_{\Delta{\bm{t}}_{ij}}^{T}]^{T}\right\|_{{\bm{\Sigma}}^{\mathrm{imu}}}^{2},$
(22)

where

$\displaystyle{\bm{r}}_{\Delta{\bm{R}}_{ij}}$
$\displaystyle=(\Delta\tilde{{\bm{R}}}_{ij}({\bm{b}}_{i}^{\rm
g}))^{\vee}{\bm{R}}_{i}^{T}{\bm{R}}_{i},$ (23)
$\displaystyle{\bm{r}}_{\Delta{\bm{v}}_{ij}}$
$\displaystyle={\bm{R}}_{i}^{T}({\bm{v}}_{j}-{\bm{v}}_{i}-{\bm{g}}\Delta
t_{ij})-\Delta\tilde{{\bm{v}}}_{ij}({\bm{b}}_{i}^{\rm g},{\bm{b}}_{i}^{\rm
a}),$ (24) $\displaystyle{\bm{r}}_{\Delta{\bm{t}}_{ij}}$
$\displaystyle={\bm{R}}_{i}^{T}({\bm{t}}_{j}-{\bm{t}}_{i}-{\bm{v}}_{i}\Delta{t}_{ij}-\frac{1}{2}{\bm{g}}\Delta{t}_{ij}^{2})-\Delta\tilde{{\bm{t}}}_{ij}({\bm{b}}_{i}^{\rm
g}\leavevmode\nobreak\ {\bm{b}}_{i}^{\rm a}).$ (25)

In (23) to (25), we omit the bias derivatives that can be ignored between two
state variables. The motion increments
$\\{\Delta\tilde{{\bm{R}}}_{ij},\leavevmode\nobreak\
\Delta\tilde{{\bm{v}}}_{ij},\leavevmode\nobreak\
\Delta\tilde{{\bm{t}}}_{ij}\\}$ are provided by the IMU pre-integration with

$\displaystyle\Delta\tilde{{\bm{R}}}_{ij}$
$\displaystyle=\prod_{k=i}^{j-1}((\tilde{{\bm{\omega}}}_{k}-{\bm{b}}_{i}^{\rm
g})\Delta t)^{\land},$ (26) $\displaystyle\Delta\tilde{{\bm{v}}}_{ij}$
$\displaystyle=\sum_{k=i}^{j-1}\Delta\tilde{{\bm{R}}}_{ik}(\tilde{{\bm{a}}}_{k}-{\bm{b}}_{i}^{\rm
a})\Delta t,$ (27) $\displaystyle\Delta\tilde{{\bm{t}}}_{ij}$
$\displaystyle=\sum_{k=i}^{j-1}\Big{[}\Delta{\bm{v}}_{ik}\Delta
t+\frac{1}{2}\Delta\tilde{{\bm{R}}}_{ik}(\tilde{{\bm{a}}}_{k}-{\bm{b}}_{i}^{\rm
a})\Delta t^{2}\Big{]},$ (28)

where the raw vehicle acceleration $\tilde{{\bm{a}}}$ and rotation rate
$\tilde{{\bm{\omega}}}$ from the IMU are integrated. The pre-defined noise
parameters $\\{{\bm{\eta}}_{\rm a},\leavevmode\nobreak\ {\bm{\eta}}_{\rm
g}\\}$ are propagated to acquire the covariance matrix
${\bm{\Sigma}}^{\mathrm{imu}}$ [58]. The gravity vector is updated according
to the current position in the $e$ frame for each pre-integration.

As in [58], we estimate the accelerometer and gyroscope biases with the
Brownian motion model by formulating the bias error function as

$\displaystyle\left\|{\bm{e}}_{ij}^{b}\right\|^{2}=\left\|{\bm{b}}_{j}^{\rm
a}-{\bm{b}}_{i}^{\rm a}\right\|_{{\bm{\Sigma}}^{\rm
a}}^{2}+\left\|{\bm{b}}_{j}^{\rm g}-{\bm{b}}_{i}^{\rm
g}\right\|_{{\bm{\Sigma}}^{\rm g}}^{2}.$ (29)

####  V-B2 Between-Pose Factor

For the relative odometry observations
$\Delta\tilde{{\bm{T}}}_{i,j}^{e}=\\{\Delta\tilde{{\bm{R}}}_{i,j}^{e}\leavevmode\nobreak\
\Delta\tilde{{\bm{p}}}_{i,j}^{e}\\}$, we follow the original implementation in
[60] and formulate the between pose factor represented as

$\left\|{\bm{e}}_{i,j}^{\mathrm{bp}}\right\|^{2}=\left\|\ln({\bm{T}}_{i}^{e,-1}{\bm{T}}_{j}^{e}\Delta\tilde{{\bm{T}}}_{i,j}^{e})^{\vee}\right\|_{{\bm{\Sigma}}^{\mathrm{bp}}}^{2},$
(30)

where the pose ${\bm{T}}_{i}^{e}$ and ${\bm{T}}_{j}^{e}$ are queried using
timestamps associated with two successive lidar scans.

####  V-B3 Velocity Factor

We use the 2D observations $\tilde{{\bm{v}}}_{t}^{b}$ to formulate the
navigation velocity factor. As the measured velocity can be directly
associated with the velocity in state variables, as denoted in (20), we
formulate the error function for the velocity observations considering the
lever arm ${\bm{l}}^{b,\mathrm{vel}}$ from the body frame to the sensor center
as

$\displaystyle\left\|{\bm{e}}_{i}^{\mathrm{vel}}\right\|^{2}=\left\|\left[\begin{matrix}1&0&0\\\
0&1&0\\\
\end{matrix}\right]\cdot({\bm{R}}_{e,i}^{b}{\bm{v}}_{b,i}^{e}+{\bm{\omega}}_{i}^{b\wedge}{\bm{l}}^{b,\mathrm{vel}})-\tilde{{\bm{v}}}_{i}^{b}\right\|_{{\bm{\Sigma}}^{\mathrm{vel}}}^{2}.$
(31)

####  V-B4 GNSS-PVT Factor

We propose a generalized implementation of the GNSS-PVT factor for the
observed antenna position $\tilde{{\bm{t}}}_{\mathrm{ant}}^{e}$ and the
velocity $\tilde{{\bm{v}}}_{\mathrm{ant}}^{n}$. Taking into account the lever
arm ${\bm{l}}_{\mathrm{ant}}^{b}$ from the IMU center to the phase center of
the GNSS antenna, we calculate the antenna position at timestamp $t_{i}$ as
${\bm{t}}_{\mathrm{ant},i}^{e}={\bm{t}}_{b,i}^{e}+{\bm{R}}_{b,i}^{e}{\bm{l}}_{\mathrm{ant}}^{b}$
and velocity as
${\bm{v}}_{\mathrm{ant},i}^{e}={\bm{v}}_{b,i}^{e}+{\bm{R}}_{b,i}^{e}({\bm{\omega}}_{i}^{b})^{\wedge}{\bm{l}}_{\mathrm{ant}}^{b}$.
Thus, the error function can be derived as

$\displaystyle\left\|{\bm{e}}_{i}^{\mathrm{pvt}}\right\|^{2}=\left\|[{\bm{r}}_{{\bm{t}}_{i}}^{T}\leavevmode\nobreak\
{\bm{r}}_{{\bm{v}}_{i}}^{T}]^{T}\right\|_{{\bm{\Sigma}}^{\mathrm{pvt}}}^{2},$
(32)

with

$\displaystyle{\bm{r}}_{{\bm{t}}_{i}}$
$\displaystyle={\bm{t}}_{\mathrm{ant},i}^{e}-\tilde{{\bm{t}}}_{\mathrm{ant},i}^{e},$
(33) $\displaystyle{\bm{r}}_{{\bm{v}}_{i}}$
$\displaystyle={\bm{R}}_{e,i}^{n}{\bm{v}}_{\mathrm{ant},i}^{e}-\tilde{{\bm{v}}}_{\mathrm{ant},i}^{n},$
(34)

where the rotation matrix ${\bm{R}}_{e,i}^{n}$ can be calculated using the
direction cosine matrix with the current geodetic coordinate
$(\varphi_{i},\leavevmode\nobreak\ \lambda_{i})$ as

$\displaystyle{\bm{R}}_{e,i}^{n}=\left[\begin{matrix}-\sin{\lambda_{i}}&-\cos{\lambda_{i}}\sin{\varphi_{i}}&\cos{\lambda_{i}}\sin{\varphi_{i}}\\\
\cos{\lambda_{i}}&-\sin{\lambda_{i}}\sin{\varphi_{i}}&\sin{\lambda_{i}}\sin{\varphi_{i}}\\\
0&\cos{\varphi_{i}}&\sin{\varphi_{i}}\end{matrix}\right].$ (35)

####  V-B5 Pseudorange and Deltarange (PrDr) Factor

We derive the error function for the pre-processed pseudorange and deltarange
observations with (17) and (18) as

$\left\|{\bm{e}}_{i}^{\mathrm{PrDr}}\right\|^{2}=\left\|[{r}_{i}^{\mathrm{Pr}}\leavevmode\nobreak\
{r}_{i}^{\mathrm{Dr}}]^{T}\right\|_{{\bm{\Sigma}}^{\mathrm{PrDr}}}^{2},$ (36)

where

$\displaystyle{r}_{i}^{\mathrm{Pr}}$
$\displaystyle=\left\|{\bm{t}}_{\mathrm{ant},i}^{e}-{\bm{t}}_{s,i}^{e,k}\right\|+c_{b,i}-\tilde{{\rho}}_{i}^{k},$
(37) $\displaystyle{r}_{i}^{\mathrm{Dr}}$
$\displaystyle=\mathbf{u}_{\mathrm{ant},i}^{s,k,T}\left({\bm{v}}_{\mathrm{ant},i}^{e}-{\bm{v}}_{s,i}^{e,k}\right)+c_{d,i}-\tilde{{\nu}}_{i}^{k}.$
(38)

In (37) and (38), the vectors ${\bm{t}}_{s,i}^{e,k}$ and
${\bm{v}}_{s,i}^{e,k}$ are the position and velocity of $k$-th satellite in
frame $e$, respectively. The unit vector $\mathbf{u}_{\mathrm{ant},i}^{s,k}$
denotes the direction from the antenna to the $k$-th satellite. The receiver
clock bias ${c}_{b,i}$ and drift ${c}_{d,i}$ are also evaluated in (36).

We consider a scaled Carrier-to-Noise ratio $(C/N_{0})$ with hyper-parameters
$\lambda_{\rho}$ and $\lambda_{\nu}$ to represent the variance of pseudorange
and deltarange observations, which is denoted as

$\displaystyle{\eta}_{\rho}^{2}=\lambda_{\rho}10^{-\frac{\mathrm{C/N}_{0}}{10}},\leavevmode\nobreak\
{\eta}_{\nu}^{2}=\lambda_{\nu}10^{-\frac{\mathrm{C/N}_{0}}{10}}.\leavevmode\nobreak\
$ (39)

Due to the strong corruption of GNSS observations in urban areas, we use
m-Estimator [61] to enhance the optimization robustness by reforming the error
function ${\bm{e}}_{i}^{\mathrm{PrDr}}$ into

$\displaystyle\hat{{\bm{e}}}_{i}^{\mathrm{PrDr}}=\phi({\bm{e}}_{i}^{\mathrm{PrDr}}),$
(40)

where the robust error formulation $\phi(\cdot)$ can be defined with different
loss functions such as Huber or Cauchy [62].

####  V-B6 GNSS Receiver Clock Error Factor

In the tight coupling of the raw GNSS observations, the unknown receiver clock
bias and drift (cbd) are estimated in the state variable by assuming a
constant drifting model, which can be fused as

$\left\|{\bm{e}}_{i}^{\mathrm{cbd}}\right\|^{2}=\left\|\left[\begin{matrix}1&\Delta
t\\\ 0&1\end{matrix}\right]\Big{[}\begin{matrix}c_{b,i-1}\\\
c_{d,i-1}\end{matrix}\Big{]}-\Big{[}\begin{matrix}c_{b,i}\\\
c_{d,i}\end{matrix}\Big{]}\right\|_{{\bm{\Sigma}}^{\mathrm{cbd}}}^{2}.$ (41)

####  V-B7 GP-WNOJ Motion Prior Factor

We implement the GP-WNOJ motion model as between-state factors, similar to
[46]. The error function was originally given in [12] using (15). We summarize
this error function for convenience as

$\displaystyle\left\|{\bm{e}}_{ij}^{\mathrm{gp}}\right\|^{2}=\left\|[{\bm{r}}_{\Delta{\bm{\gamma}}_{ij}}^{T}\leavevmode\nobreak\
{\bm{r}}_{\Delta{\bm{\varpi}}_{ij}}^{T}]^{T}\right\|_{{\bm{\Sigma}}^{\mathrm{gp}}}^{2},$
(42)

where

$\displaystyle{\bm{r}}_{\Delta{\bm{\gamma}}_{ij}}$
$\displaystyle=\ln({\bm{T}}_{j,i})^{\lor}-(t_{j}-t_{i}){\bm{\varpi}}_{i}-\frac{1}{2}(t_{j}-t_{i})^{2}\dot{{\bm{\varpi}}}_{i},$
(43) $\displaystyle{\bm{r}}_{\Delta{\bm{\varpi}}_{ij}}$
$\displaystyle=\bm{\mathcal{J}}_{j,i}^{-1}{\bm{\varpi}}_{j}-{\bm{\varpi}}_{i}-(t_{j}-t_{i})\dot{{\bm{\varpi}}}_{i}.$
(44)

As introduced in Sec. V-A, we used the measured accelerations of the IMU in
our GP motion models. Thus, only the 6D pose and the 6D velocity are evaluated
in GP-WNOJ motion factors, so that
${\bm{e}}_{ij}^{\mathrm{gp}}\in\mathbb{R}^{12}$. The analytical Jacobians of
the GP motion models can be found in [63, 12].

###  V-C Loosely Coupled FGO

Although the loosely coupled fusion with GNSS and IMU measurements is shown to
be less performant compared to tight coupling [6], we implemented a loosely
coupled fusion of sensor observations to i) propose a baseline trajectory by
integrating a high-grade speed sensor for our dataset in urban areas, where an
RTK-fixed GNSS-PVA solution is unreliable; ii) evaluate the loosely and
tightly coupled fusion for GNSS-based vehicle localization in challenging
areas; iii) demonstrate the flexibility and scalability of the proposed
method.

The proposed factor graph is shown in Fig. 5. The states ${\bm{x}}_{1:t}$ are
created deterministically on the graph independently of any measurement. If a
measurement cannot be associated with any state variable, a state
$\hat{{\bm{x}}}_{i+\tau}$ between two state variables $\hat{{\bm{x}}}_{i}$ and
$\hat{{\bm{x}}}_{i+1}$ (where $0<\tau<1$) is queried for the error evaluation.

Figure 5: Proposed loosely coupled GNSS-FGO.

The optimization problem can then be formulated as

$\begin{split}\hat{{\bm{x}}}&=\arg\\!\min_{{\bm{x}}}\Big{(}\left\|{\bm{e}}^{0}\right\|_{{\bm{\Sigma}}_{0}}^{2}+\sum_{i=1}^{M}\left\|{\bm{e}}_{i}^{\mathrm{imu}}\right\|_{{\bm{\Sigma}}^{\mathrm{imu}}}^{2}+\sum_{i=1}^{M}\left\|{\bm{e}}_{i}^{\mathrm{gp}}\right\|_{{\bm{\Sigma}}^{\mathrm{gp}}}^{2}+\\\
&+\sum_{i=1}^{N}\left\|{\bm{e}}_{i}^{\mathrm{vel}}\right\|_{{\bm{\Sigma}}^{\mathrm{vel}}}^{2}+\sum_{i=1}^{K}\left\|{\bm{e}}_{i}^{\mathrm{pvt}}\right\|_{{\bm{\Sigma}}^{\mathrm{pvt}}}^{2}+\sum_{i=1}^{J}\left\|{\bm{e}}_{i}^{\mathrm{bp}}\right\|_{{\bm{\Sigma}}^{\mathrm{bp}}}^{2}\Big{)},\end{split}$
(45)

where the error term ${\bm{e}}^{0}$ represents the prior factor obtained at
initialization or from marginalization. Because sensor observations are
received asynchronously other than estimation timestamps $M$, we use different
index notations $N,\leavevmode\nobreak\ K,\leavevmode\nobreak\ J$ to indicate
the number of sensor observations in (45).

###  V-D Tightly Coupled FGO

In contrast to the loosely coupled fusion approach, a tightly coupled fusion
of raw GNSS observations contributes more constraints with multiple observed
satellites to state variables, as illustrated in Fig. 6. Unlike Fig. 5, we
include the pseudorange and deltarange factors in the graph, providing
redundant constraints to each state variable. To improve the robustness while
GNSS observations are degraded or lost in challenging areas, we include lidar
odometry as between-state constraints to improve the consistency of the
estimated trajectory. The receiver clock error factor is also added to the
graph.

Figure 6: Proposed tightly coupled GNSS-FGO.

The optimization problem with sensor observations from different time domains
becomes

$\begin{split}\hat{{\bm{X}}}=\arg\\!\min_{{\bm{x}}}\Big{(}&\left\|{\bm{e}}_{i}^{0}\right\|_{{\bm{\Sigma}}_{0}}^{2}+\sum_{i=1}^{M}\left\|{\bm{e}}_{i}^{\mathrm{IMU}}\right\|_{{\bm{\Sigma}}^{\mathrm{IMU}}}^{2}+\sum_{i=1}^{M}\left\|{\bm{e}}_{i}^{\mathrm{gp}}\right\|_{{\bm{\Sigma}}^{\mathrm{gp}}}^{2}+\\\
&+\sum_{i=1}^{N}\left\|{\bm{e}}_{i}^{\mathrm{bp}}\right\|_{{\bm{\Sigma}}^{\mathrm{bp}}}^{2}+\sum_{i=1}^{M}\left\|{\bm{e}}_{i}^{\mathrm{cbd}}\right\|_{{\bm{\Sigma}}^{\mathrm{cbd}}}^{2}+\\\
&+\sum_{i=1}^{J}\sum_{s=1}^{K}\left\|\hat{{\bm{e}}}_{s,i}^{\mathrm{PrDr}}\right\|_{\phi({\bm{\Sigma}}^{\mathrm{PrDr}})}^{2}\Big{)}.\end{split}$
(46)

###  V-E System Overview

The system overview with the implementation of Alg. 1 and all data interfaces
is shown in Fig. 7. The sensor data are received and pre-processed in separate
processes. We construct the time-centric factor graph in a two-stage process,
as introduced in Alg. 1. The first stage (line 4-10 of Alg. 1) includes
between-state factors and delay-free IMU factors to build a deterministic
graph on time. Subsequently, asynchronous sensor observations are fused into
the deterministic graph by aligning the timestamps between the measurement and
the state variables (line 11-24 of Alg. 1). For measurements that cannot be
aligned with any state, two successive state variables are queried to
construct a GP-interpolated state for measurement evaluation in optimization
procedures. The time-centric graph can be optimized using a fixed-lag batch
optimizer [64] or a fixed-lag incremental smoother iSAM2 [65] at a lower
frequency. In the experimental results, the estimated trajectories in the
error metrics are optimized using iSAM2. We also evaluate both smoothers with
respect to both estimator performance and computation efficiency, as presented
in Sec. VII-D. After each optimization procedure, we forward the optimized
state variables to a state publisher and sensor pre-processing modules. The
state publisher is associated with the IMU sensor and provides high-frequent
state estimates at $200\text{\,}\mathrm{H}\mathrm{z}$.

Figure 7: System overview showing all data interfaces and factor types by
implementing Alg. 1.

######  Remark 3.

Near-Zero-Velocity Detection: While the vehicle is stationary, the state
estimation exhibits random pose drift. This is a known problem in vehicle
localization using inertial measurements [66]. In this case, the state
observability degrades dramatically due to insufficient IMU excitation,
leading to unbounded error accumulation. Thus, we follow the idea proposed in
[66] to detect near-zero velocity motion by voting through multiple sensors
that provide velocity information. If the vehicle is voted to be stationary,
we temporally pause the graph optimization and state propagation.

###  V-F Implementation

We implemented our approach in C++ using Robot Operating System
ROS2666https://docs.ros.org/en/rolling/index.html. The open-source software
library GTSAM777https://gtsam.org was extended to implement the graph and
factor formulations. We adopted the software solution for lidar odometry from
LIO-SAM777https://gtsam.org, where only the front-end feature extraction and
association were adapted in our work. We used the positioning and orientation
estimation solution from a dual-antenna GNSS setup to initialize the state
variable ${\bm{x}}_{0}$. In this work, we used a laptop with an Intel
i9-9900K, $16\text{\,}\mathrm{c}\mathrm{o}\mathrm{r}\mathrm{e}\mathrm{s}$ at
max. $4.7\text{\,}\mathrm{G}\mathrm{Hz}$ and $64\text{\,}\mathrm{G}\mathrm{B}$
memory for sensor pre-processing and graph optimization in experimental
studies.

88footnotetext: https://github.com/TixiaoShan/LIO-SAM

##  VI Measurement Setup and Test Sequences

###  VI-A Measurement Setup

In the measurement campaigns, we recorded sensor data of long-range routes in
different areas of Aachen, Düsseldorf and Cologne. Our sensor setup included a
dual-antenna GNSS receiver (Novatel PwrPark7D), a Microstrain 3DM-GX5 IMU, and
a Velodyne VLP-16 lidar. A high-grade optical speed sensor, Correvit S-Motion
DTI from Kistler, was mounted on the trailer hitch at the vehicle’s rear side.
The test vehicle with sensors is shown in Fig. 8.

The IMU data were acquired at $200\text{\,}\mathrm{Hz}$, while the GNSS
observations and lidar pointclouds were recorded at $10\text{\,}\mathrm{Hz}$.
We used the high-grade GNSS receiver Novatel PwrPark7D with a dual
constellation of GPS and Galileo satellite systems as a reference source. In
addition to the sensor data, we received the 1PPS signal at
$1\text{\,}\mathrm{Hz}$ to calculate the measurement delays. The
$\mathrm{RTCMv3}$ correction data from German satellite positioning service
SAPOS999http://www.sapos.nrw.de was also stored at about
$1\text{\,}\mathrm{Hz}$ for GNSS pre-processing.

Figure 8: Sensor setup and frames on the test vehicle.

###  VI-B Test Sequences

Our dataset contains different driving scenarios: open-sky, semi-/dense-urban,
and high-speed track. For a clear evaluation, we define different test
sequences throughout multiple measurement campaigns and analyze the driving
conditions for each sequence, as shown in Table I. The test sequences include
lengthy runs with a max. $17\text{\,}\mathrm{km}$ route, aiming to evaluate
the estimation performance for long-term operations. For test sequences in
urban areas, we chose data from scenarios with different urbanization rates
containing tunnel and bridge crossings to evaluate the limitations of the
proposed fusion approaches. In addition, we also considered open-sky areas on
the high-speed track, where a maximum vehicle speed of
$170\text{\,}\mathrm{k}\mathrm{m}\mathrm{/}\mathrm{h}$ was reached, creating
significant motion distortion in the lidar point clouds.

TABLE I: Test sequences definition. We denote the test sequences in Aachen,
Düsseldorf, Cologne, and high-speed tracks with “AC”, “DUS”, “C”, and “HS”,
respectively. The variable $\bar{{v}}$ represents the average speed and the
scalar $\bar{{n}}^{\mathrm{sat.}}$ is the average number of satellites used
for a GNSS-PVA solution. We calculated the ratio of RTK-fixed solution and No-
Solution due to insufficient GNSS observations denoted by
${R}_{\mathrm{fixed}}^{\mathrm{RTK}}$ and ${R}^{\mathrm{NS}}$,
correspondingly.

Seq. |  | Leng.
---
($\mathrm{km}$)
| Tunnel
---
($\mathrm{m}$)
| Dura.
---
($\mathrm{s}$)
| $\bar{{v}}$
---
($\mathrm{k}\mathrm{m}\mathrm{/}\mathrm{h}$)
$\bar{{n}}^{\mathrm{sat.}}$ |  | ${R}_{\mathrm{fixed}}^{\mathrm{RTK}}$
---
($\mathrm{\char 37\relax}$)
| ${R}^{\mathrm{NS}}$
---
($\mathrm{\char 37\relax}$)
AC | 17.0 | 270 | 2477 | 27.25 | 11 | 76.51 | 1.7
DUS | 5.25 | - | 1350 | 13.48 | 8 | 52.06 | 0.9
C01 | 0.81 | 276 | 160 | 17.89 | 7 | 60.8 | 31.78
C02 | 1.45 | 145 | 390 | 13.36 | 7 | 37.74 | 11.56
HS | 10.6 | - | 300 | 124.82 | 14 | 94.9 | 1.47

###  VI-C Reference Trajectory and Metrics

To evaluate the proposed fusion strategies, we employ the RTK-fixed GNSS-PVA
solution associated with low uncertainties ($\sigma_{\rm
pos}<$0.05\text{\,}\mathrm{m}$$ and $\sigma_{\rm
rot}<$1\text{\,}\mathrm{\SIUnitSymbolDegree}$$) to calculate the absolute root
mean square error (RMSE).

Besides the error metrics, we employ Pythagoras’ theorem implemented in the
Open Motion Planning Library101010https://ompl.kavrakilab.org/ (OMPL) to
calculate the trajectory smoothness (contrary to trajectory roughness) for all
test sequences. The smoothness is given as the sum of angles between all path
segments in the local-world frame, as denoted in (47), where the variables
$a_{i}$, $b_{i}$ and $c_{i}$ are the length of trajectory segments containing
three successive vehicle positions in the Euclidean frame. For the same test
sequence with $k$ vehicle positions, a smaller $s$ shows a large smoothness of
the trajectory.

$s=\sum_{i=2}^{k-1}\Bigg{(}\frac{2(\pi-\arccos{\frac{a_{i}^{2}+b_{i}^{2}-c_{i}^{2}}{2a_{i}b_{i}}})}{a_{i}+b_{i}}\Bigg{)}^{2}.$
(47)

##  VII Experiments and Results

###  VII-A Experiment Setting

To evaluate the proposed GNSS-FGO, we first benchmarked the loosely coupled
fusion of the GNSS solution with multiple sensor observations using the
proposed GNSS-FGO with a lidar-centric SLAM approach LIO-SAM [2], aiming to
evaluate the robustness of the proposed method. For a fair evaluation, we have
adapted the LIO-SAM implementation66footnotemark: 6 using the same robust
error models and parameterizations as in our method. We also enabled loop-
closure detection in LIO-SAM to maximize state estimation performance. Later,
we conducted experiments by fusing raw GNSS observations alongside IMU and
lidar measurements in a tight coupling, which is expected to present more
robust trajectory estimation in challenging areas compared to the loose
coupling. Lastly, we discussed the smoother type and computation time using
different lag sizes. We also evaluated the GP-WNOJ prior with the GP-WNOA
prior.

###  VII-B General Error Metrics

With pre-defined test sequences in Table I, we present the general error
metrics for all experiments in Table II by taking the RTK-fixed GNSS-PVA
solution as ground truth. Because an RTK-fixed solution is unavailable in
challenging areas, we denote the solution rate used as a ground-truth
reference to calculate error metrics of each test sequence in percentage in
the column “Seq.”.

TABLE II: General Trajectory Estimation Metrics. The rate of RTK-fixed GNSS-PVA solution used for the error metrics is denoted in the first column. A test run is classified as failed if the algorithm diverges. Seq. | Configuration |  | Mean 2D
---
Pos. Err.
(m)
| 2D Pos.
---
STD
(m)
| Max. 2D
---
Pos. Err.
(m)
| Mean 3D
---
Pos. Err.
(m)
| 3D Pos.
---
STD
(m)
| Max. 3D
---
Pos. Err.
(m)
| Mean
---
Yaw Err.
(°)
| Yaw
---
STD
(°)
| Max .
---
Yaw Err.
(°)
$s$
AC (52%) | LIO-SAM [2] | Failed
GNSS-FGO-lc | 0.188 | 0.052 | 5.653 | 0.385 | 0.075 | 17.61 | 0.36 | 1.03 | 86.14 | 5108.23
| GNSS-FGO-tc
---
w/o. LiDAR
0.936 | 0.650 | 5.877 | 1.276 | 1.392 | 6.127 | 2.76 | 2.68 | 88.53 | 1706.63
| GNSS-FGO-tc
---
w. LiDAR
0.476 | 0.738 | 1.605 | 1.057 | 1.088 | 2.343 | 1.18 | 2.13 | 85.12 | 982.40
DUS (18%) | LIO-SAM [2] | 0.7359 | - | 9.129 | 1.024 | - | 17.391 | 1.758 | - | 80.44 | 2.15e+5
GNSS-FGO-lc | 0.397 | 0.028 | 6.821 | 1.146 | 0.042 | 19.36 | 0.216 | 0.44 | 30.64 | 5672.83
| GNSS-FGO-tc
---
w/o. LiDAR
1.123 | 1.081 | 4.415 | 1.461 | 2.524 | 7.141 | 0.473 | 3.01 | 81.32 | 3595.72
| GNSS-FGO-tc
---
w. LiDAR
1.074 | 0.921 | 4.390 | 2.170 | 1.890 | 5.742 | 0.492 | 2.74 | 47.867 | 3260.06
C01 (57%) | LIO-SAM [2] | 3.339 | - | 8.186 | 3.478 | - | 8.463 | 3.774 | - | 34.260 | 6.30e+5
GNSS-FGO-lc | 0.043 | 0.013 | 0.875 | 0.047 | 0.022 | 1.271 | 0.160 | 0.51 | 4.069 | 144.077
| GNSS-FGO-tc
---
w/o. LiDAR
0.516 | 1.395 | 0.903 | 1.223 | 2.849 | 2.336 | 2.134 | 2.43 | 7.567 | 1.52e+5
| GNSS-FGO-tc
---
w. LiDAR
0.402 | 0.846 | 0.807 | 0.468 | 1.892 | 0.841 | 0.888 | 2.76 | 7.025 | 122.358
C02 (27%) | LIO-SAM [2] | Failed
GNSS-FGO-lc | 0.010 | 0.010 | 0.164 | 0.014 | 0.023 | 0.165 | 0.085 | 0.79 | 1.146 | 2.15e+6
| GNSS-FGO-tc
---
w/o. LiDAR
0.933 | 1.111 | 1.933 | 1.188 | 2.836 | 2.104 | 0.927 | 4.15 | 3.811 | 198.03
| GNSS-FGO-tc
---
w. LiDAR
0.264 | 1.039 | 0.499 | 0.328 | 2.402 | 0.565 | 0.496 | 2.40 | 9.716 | 218.42
HS (78%) | LIO-SAM [2] | Failed on Initialization
GNSS-FGO-lc | 0.03 | 0.083 | 0.944 | 0.263 | 0.05 | 1.562 | 0.09 | 0.33 | 1.57 | 0.0125
| GNSS-FGO-tc
---
w/o. LiDAR
0.404 | 0.509 | 2.265 | 0.822 | 1.08 | 4.833 | 0.993 | 1.69 | 10.05 | 0.1388
| GNSS-FGO-tc
---
w. LiDAR
0.475 | 0.672 | 3.121 | 1.400 | 1.252 | 3.609 | 0.699 | 0.94 | 3.778 | 0.060

(a) 2D Trajectories of Seq. AC.

(b) Coordinates in the WGS84 frame.

(c) Estimated rotation in the NED frame.

(d) Estimated velocity in the NED frame.

Figure 9: Trajectory plot ($700\text{\,}\mathrm{s}$ \-
$1400\text{\,}\mathrm{s}$) in urban areas in Aachen. We plot the GNSS single
point position (SPP) if the RTK-fixed solution is unavailable.

(a) 2D Trajectories of Seq. DUS.

(b) Coordinates in the WGS84 frame.

(c) Estimated rotation in the NED frame.

(d) Estimated velocity in the NED frame.

Figure 10: Trajectory plot ($450\text{\,}\mathrm{s}$ \-
$1350\text{\,}\mathrm{s}$) in challenging areas in Düsseldorf.

(a) 2D Trajectories of Seq. C01.

(b) Coordinates in the WGS84 frame.

(c) Estimated rotation in the NED frame.

(d) Estimated velocity in the NED frame.

Figure 11: Trajectory plot near the central station of Cologne. For the
tightly coupled fusion, we omitted the near-zero-velocity detection in order
to present the trajectory drifting while the receiver clock error is
unobservable.

####  VII-B1 Lidar-Centric Fusion

As shown in Table II, the lidar-centric SLAM approach LIO-SAM failed in
several test sequences even when the same factors with robust error modeling
were used and loop-closure detection was enabled (see video
demonstration22footnotemark: 2). The most frequent reason is that scan
registration fails due to an invalid feature association, which can be
observed in all failed test sequences. Fig. 1a demonstrates this result, where
the estimation diverged and cannot be recovered after the vehicle entered a
tunnel. In Seq. HS, the lidar-centric approach cannot even be initialized
properly while the vehicle was moving very fast, which was not observed in the
proposed GNSS-FGO.

We conjecture that because graph construction in [2] requires strict timestamp
synchronization of GNSS measurements with lidar timestamps, asynchronous GNSS
measurements are dropped, resulting in information loss, and thus, the
trajectory smoothness and estimation accuracy are dramatically penalized (see
Table II). This hypothesis is supported in the test sequences DUS and C01 (see
Fig. 10 and Fig. 11), where the estimated height, orientation, and velocities
were frequently diverted.

####  VII-B2 Loosely Coupled (lc) Fusion

Because the position and velocity can be directly observed by fusing the GNSS
solution in the loose coupling, the to-be-estimated state variables are
effectively constrained, resulting in highly accurate trajectory estimation in
open-sky areas. For instance, Seq. C01 and C02 present mean position errors
less than $5\text{\,}\mathrm{cm}$, while the max. position errors remain below
$1\text{\,}\mathrm{m}$. For long test sequences (e.g., Seg. AC), the loosely
coupled fusion using the proposed GNSS-FGO also presents sufficient estimation
performance by integrating multiple sensor observations. However, a fast
divergence and larger max. position errors can be observed in challenging
scenarios using this fusion mechanism. As shown in Fig. 9b and Fig. 10b, the
estimated height (in red) diverges significantly once the GNSS positioning is
corrupted. This result also proves that the 2D velocity measurements provided
by the 2D speed sensor cannot sufficiently constrain the state space. For
state variables such as the vertical velocity component ${v}_{d}^{n}$ that are
observed with noisy measurements, frequent drifting can be expected (see Fig.
9d). Moreover, the loose coupling generally presents larger trajectory
roughness compared to other configurations. Compared to the Seq. AC (see Fig.
9), a higher urbanization rate can be expected in Düsseldorf, leading to an
unsmooth trajectory estimation.

####  VII-B3 Tightly Coupled (tc) Fusion

Compared to loosely coupled sensor fusion, integrating pre-processed GNSS
observations in a tight coupling contributes to redundant state constraints.
Thus, the tightly coupled fusion can generally present more robust trajectory
estimations with a smaller max. position error and larger trajectory
smoothness in lengthy runs, except in the high-speed scenario (Seq. HS).
However, because the vehicle pose and velocity cannot be directly observed
with pseudorange and deltarange, lower accuracy can be expected. In
challenging urban areas such as Seq. C01 and C02, fusing the lidar odometry as
between-state constraints generally improves the estimation performance and
trajectory smoothness. This conclusion can also be drawn when referring to
Fig. 10b and Fig. 11b, where more accurate height and velocity estimations can
be observed by fusing lidar odometry in the graph. In high-speed scenarios,
lidar scans suffer from serious motion distortion, and no sufficient features
can be extracted compared to urban areas. Therefore, a limited performance
improvement can be observed by fusing lidar odometry in the graph.

####  VII-B4 Discussion

Based on the experimental results presented above, a robust trajectory
estimation can be achieved in challenging scenarios using the proposed
approach by fusing multiple sensor measurements, which supports our hypothesis
proposed in Sec. II. On the contrary, it can be observed that trajectory drift
cannot be effectively eliminated using the classic sensor-centric localization
approach LIO-SAM. Even worse, the robustness and reliability of sensor-centric
approaches cannot be guaranteed in challenging areas once the primary sensor
is compromised. As online applications raise requirements on computation time
and resources, sensor degradation due to, e.g., insufficient data processing,
becomes nontrivial. The proposed GNSS-FGO presents an effective workaround
while fusing multiple sensors to discharge dependence on a single sensor,
which enables lossless information fusion and improves the robustness of the
estimation if sensor failure can be expected.

Within GNSS-FGO, loosely fusing the GNSS positioning solution enables fast
estimation convergence and higher accuracy in open-sky areas compared to
tightly coupled fusion in our study. The loosely coupled fusion diverges
quickly once the vehicle enters challenging areas, even when more sensor
modalities are integrated. In contrast, the tightly coupled multi-sensor
fusion presents a more robust trajectory estimation in our experimental
studies. The same conclusion has also been shown in [6, 32]. However, an
acceptable accuracy cannot be achieved, especially in dense urban scenarios.
For instance, although all estimated trajectories using the proposed GNSS-FGO
in Fig. 1 remain consistent, a large drift is presented using the proposed
sensor integration. One possible reason can be traced back to lidar
degradation and insufficient outlier rejection in GNSS observations. As GNSS-
FGO provides a flexible fusion mechanism, this problem can be addressed by
integrating more effective state constraints into the graph.

###  VII-C Challenging Scenarios

In this part, we propose experimental studies regarding GNSS observations,
lidar odometry, and solver settings. We also evaluated the GP-WNOA and GP-WNOJ
priors and discussed the hyper-parameter tuning for ${\bm{Q}}_{c}$.

####  VII-C1 Loss of GNSS Observation

Generally, losing GNSS observations in a short time interval does not lead to
immediate divergence or trajectory drift if multiple state constraints such as
lidar odometry or motion prior factors are still presented. This conclusion
can be drawn from our experiment in Seq. C01, where the vehicle crossed a
large bridge at the central train station in Cologne, as shown in Fig. 11.
However, fusing GNSS observations in a tight coupling extends the state
variables with receiver clock bias and drift
${\bm{c}}_{r}=[{c}_{b}\leavevmode\nobreak\ {c}_{d}]^{T}$, which become
unobservable if less than four satellites are visible. Fig. 12 shows the
estimated clock bias ${c}_{b}$ with respect to the number of received
satellites. In the graph where only GNSS observations are integrated, the
estimated clock bias drifts dramatically, which is not observed in the graph
fused with the lidar odometry. Similar results can also be observed in other
experiments, in which the unobservable state variables can lead to estimation
divergence and an ill-posed optimization problem. Furthermore, if the global
reference (e.g., GNSS observations) is lost over a long time interval, such as
crossing a long tunnel, a large trajectory drift can be expected.

Figure 12: Estimated GNSS receiver clock bias of Seq. C01 without near-zero-
velocity detection. The receiver clock error is unobservable during tunnel
crossing. In this scenario, fusing raw GNSS observations without lidar
odometry cannot constrain the state estimation, resulting in trajectory
drifting.

####  VII-C2 Highly Corrupted GNSS Observations

Compared to the temporary loss of GNSS observations, we emphasize that
including highly corrupted GNSS observations in the graph has a greater impact
on estimation performance. This conclusion can be supported by Seq. C02, where
the accuracy of our proposed fusion paradigms is significantly degraded in
GNSS-corrupted areas, as shown in Fig. 1. In Fig. 13, we plot the estimated
trajectories in this scenario by transforming the coordinates in the
navigation frame (ENU). A large trajectory drift up to $25\text{\,}\mathrm{m}$
can be observed in tightly coupled fusion without lidar odometry (see Fig.
13). Although fusing relative motion constraints, such as odometry, can
effectively constrain divergence, trajectory drifts cannot be eliminated until
valid global references are acquired.

Figure 13: Coordinates in the ENU frame of Seq. C02. The estimated
trajectories become unsmooth if the GNSS observations are strongly corrupted
in urban areas.

####  VII-C3 Lidar Odometry Degradation

As discussed in [67], traditional lidar odometry algorithms suffer from
dramatic degradation in unstructured environments and high-speed scenarios.
This problem can also be observed in our experiments. Fig. 14 illustrates
three scenarios in which the accuracy of the lidar odometry is penalized if
the vehicle is driving in featureless areas or in high-speed mode with an
average vehicle speed of
$125\text{\,}\mathrm{k}\mathrm{m}\mathrm{/}\mathrm{h}$. In low-speed driving
mode and open-sky areas, lidar degradation does not reduce the estimation
performance while high-quality GNSS measurements are available. However, if
the vehicle is moving at high speed, the lidar odometry becomes inaccurate
because of motion distortion. Therefore, including lidar odometry factors in
the graph can decrease localization accuracy, as presented in Table II of Seq.
HS. In scenarios with long tunnels, trajectory drifting can always be expected
due to loss of global reference. This presents the major limitation of classic
lidar odometers that calculate only pose increments. To overcome this
limitation, more observed state variables, such as vehicle velocity, can be
considered [67].

Figure 14: Examples of lidar odometry degradation in three scenarios: a)
unstructured feature-less area, b) high-speed scenario and c) long tunnel
($400\text{\,}\mathrm{m}$).

###  VII-D Smoother Type and Computation Time

To study the impact of different smoother types and lag sizes, we evaluated
batch and incremental smoother iSAM2 with different lag sizes for Seq. DUS.
The performance metrics are presented in Table III. Compared to an incremental
smoother, solving the optimization problem with a batch optimizer does not
show a considerable improvement in accuracy. This happens because the graph
structure becomes more similar to a Markov chain in large-scale localization
applications where fewer loop-closure constraints are available. In this
scenario, re-linearizing all past state variables does not contribute more
information that improves the accuracy. For loosely coupled fusion, the batch
smoother presents a smoother trajectory. However, this advantage is absent
with the incremental smoother when fusing GNSS observations in a tight
coupling.

Furthermore, the batch optimizer requires more computational resources than
the incremental smoother (see Fig. 15), especially in urban areas with more
measurement outliers. In online applications, estimation accuracy and
trajectory smoothness can be penalized once optimization takes longer. This
conclusion is supported by referring to the tightly coupled fusion in Table
III. Similarly to the optimizer type, considering a large lag size does not
contribute significantly. Moreover, even the incremental smoother with a large
lag size frequently violates the desired optimization frequency, resulting in
inefficient optimization procedures.

TABLE III: Estimation Performance of Seq. DUS using Different Solver Settings. Conf. |  | Mean 2D
---
Pos. Err.
(m)
| Mean 3D
---
Pos. Err.
(m)
| Mean
---
Yaw Err.
(°)
$s$
lc-batch-3s | 0.324 | 0.839 | 0.208 | 1526.4
lc-isam2-3s | 0.397 | 1.146 | 0.216 | 5672.8
tc-batch-3s | 1.543 | 2.008 | 0.497 | 1649.3
tc-isam2-10s | 1.419 | 1.733 | 0.573 | 1537.9
tc-isam2-3s | 1.524 | 1.766 | 0.511 | 1510.5
Figure 15: Computation time with different configurations of Seq. DUS.

###  VII-E GP-WNOA/WNOJ Motion Model

In this section, we evaluated the continuous-time trajectory representation
using the Gaussian process interpolation with both white-noise-on-jerk (GP-
WNOJ) and white-noise-on-acceleration (GP-WNOA) models. As introduced in Sec.
IV-D, we do not discuss hyper-parameter tuning in this work. The hyper-
parameter ${\bm{q}}_{c}$ was manually tuned by penalizing the vehicle pose in
${\bm{Q}}_{c}$ equally for both models.

Compared to the GP-WNOJ model, a GP-WNOA model assumes that the system
transition follows a constant velocity model [44, 7]. As discussed in [12],
representing vehicle trajectories with an approximately constant-velocity
model may be insufficient in urban driving scenarios where the vehicle
accelerates and brakes frequently. To evaluate the performance of both GP
models, we chose a part of Seq. AC containing $200\text{\,}\mathrm{s}$ test
run in open-sky areas where the GNSS-PVA solution presents the ground-truth
trajectory. We calculate the whitened error of the vehicle pose and the linear
velocity in the body frame and plot the results on the histogram in Fig. 16.
Because the GP-WNOJ model represents second-order system dynamics, it shows
smaller errors in all linear velocity components. Both models perform
similarly in position estimation, where the GP-WNOJ is more accurate in the
main motion direction $x\mathrm{-axis}$. For the rotation, the GP-WNOJ does
not present considerable improvements compared to the GP-WNOA. One possible
reason supporting this result can be traced back to the rotational
acceleration that cannot be observed directly using the IMU, as introduced in
Sec. V-A.

Figure 16: Histogram of whitened state errors of GP models in an open-sky
area.

We have validated that the GP motion model formulates a valid continuous-time
trajectory representation. However, tuning the power spectral matrix
${\bm{Q}}_{c}$ that scales the system transition in the Gaussian process
kernel has a large effect on numerical stability and estimation performance
[12]. Although the GP-WNOJ model presents reliable velocity estimates compared
to the GP-WNOA model, it requires more careful parameter tuning when
incorporating accelerations in state propagation. For trajectory estimation
applications that conduct multi-sensor fusion, the state variables are
generally sufficiently constrained by heterogeneous sensor factors, making the
data-driven hyper-parameter tuning possible [53].

##  VIII Conclusion

This article proposes an online factor graph optimization that generalizes
multi-sensor fusion for robust trajectory estimation with a focus on GNSS. The
vehicle trajectory is represented in continuous time using a Gaussian process
motion prior that enables arbitrary state querying, presenting a sensor-
independent graph optimization. We successfully fused asynchronous sensor
measurements, including GNSS, IMU, vehicle speed sensor, and lidar odometry,
into the proposed method for robust vehicle localization in challenging
environments. The experimental studies show that the proposed method is
robust, flexible, accurate, and works online with multiple datasets collected
from different challenging scenarios, including urban areas and high-speed
tracks. All our FGO configurations, loosely and tightly coupled with and
without lidar, succeed in all test sequences, whereas the classic state-of-
the-art lidar-centric method [2] failed in some situations due to scan
registration failures. Observed from the experimental results, the GP-WNOJ
motion prior enables accurate trajectory representations in continuous time
with properly tuned hyper-parameters. In addition, fusing GNSS observations in
a tight coupling has demonstrated improved trajectory smoothness and
estimation robustness. Future work will include extending GNSS observations
with carrier-phases, online parameter tuning, and sensor noise identification.

## Acknowledgments

The authors thank Robin Taborsky from the Institute of Automatic Control at
the RWTH Aachen University and the public order office in Aachen, Düsseldorf,
and Cologne for their great support in measurement campaigns. We also thank
David Yoon and Keenan Burnett from the Autonomous Space Robotics Laboratory at
the University of Toronto for their discussions and support in this work.

## References

  * [1] N. Zhu, J. Marais, D. Bétaille, and M. Berbineau, “GNSS position integrity in urban environments: A review of literature,” _IEEE Trans. on Intelligent Transportation Systems_ , vol. 19, no. 9, pp. 2762–2778, 2018.
  * [2] T. Shan, B. Englot, D. Meyers, W. Wang, C. Ratti, and D. Rus, “LIO-SAM: Tightly-coupled LiDAR inertial odometry via smoothing and mapping,” _2020 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS)_ , pp. 5135–5142, 2020.
  * [3] W. Burgard, O. Brock, and C. Stachniss, _Map-Based Precision Vehicle Localization in Urban Environments_ , 2008, pp. 121–128.
  * [4] G. Bresson, Z. Alsayed, L. Yu, and S. Glaser, “Simultaneous localization and mapping: A survey of current trends in autonomous driving,” _IEEE Trans on Intelligent Vehicles_ , vol. 2, no. 3, pp. 194–220, 2017.
  * [5] C. Sung, S. Jeon, H. Lim, and H. Myung, “What if there was no revisit? Large-scale graph-based SLAM with traffic sign detection in an HD map using LiDAR inertial odometry,” _Intelligent Service Robotics_ , vol. 15, pp. 161–170, 2022.
  * [6] W. Wen, T. Pfeifer, X. Bai, and L.-T. Hsu, “Factor graph optimization for GNSS/INS integration: A comparison with the extended Kalman filter,” _NAVIGATION_ , vol. 68, no. 2, pp. 315–331, 2021.
  * [7] H. Zhang, X. Xia, M. Nitsch, and D. Abel, “Continuous-Time factor graph optimization for trajectory smoothness of GNSS/INS navigation in temporarily gnss-denied environments,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 4, pp. 9115–9122, 2022.
  * [8] F. Dellaert, “Factor Graphs: Exploiting structure in robotics,” _Annual Review of Control, Robotics, and Autonomous Systems_ , vol. 4, no. 1, pp. 141–166, 2021.
  * [9] L.-T. Hsu, “Analysis and modeling GPS NLOS effect in highly urbanized area,” _GPS Solutions_ , vol. 22, 11 2017.
  * [10] K. Burnett, Y. Wu, D. J. Yoon, A. P. Schoellig, and T. D. Barfoot, “Are we ready for radar to replace lidar in all-weather mapping and localization?” _IEEE Robotics and Automation Letters_ , vol. 7, no. 4, pp. 10 328–10 335, 2022.
  * [11] J. Jeong, Y. Cho, Y.-S. Shin, H. Roh, and A. Kim, “Complex urban dataset with multi-level sensors from highly diverse urban environments,” _The Int. Journal of Robotics Research_ , vol. 38, no. 6, pp. 642–657, 2019.
  * [12] T. Y. Tang, D. J. Yoon, and T. D. Barfoot, “A White-Noise-on-Jerk motion prior for continuous-time trajectory estimation on SE(3),” _IEEE Robotics and Automation Letters_ , vol. 4, no. 2, pp. 594–601, 2019.
  * [13] J. Zhang and S. Singh, “Low-drift and real-time LiDAR odometry and mapping,” _Autonomous Robots_ , vol. 41, pp. 401–416, 02 2017.
  * [14] A. Geiger, P. Lenz, and R. Urtasun, “Are we ready for autonomous driving? the kitti vision benchmark suite,” in _Conference on Computer Vision and Pattern Recognition (CVPR)_ , 2012.
  * [15] N. Sünderhauf and P. Protzel, “Towards robust graphical models for gnss-based localization in urban environments,” in _Int. Multi-Conference on Systems, Signals & Devices_, 2012, pp. 1–6.
  * [16] R. Watson and J. Gross, “Robust navigation in GNSS degraded environment using graph optimization,” in _30th Int. Technical Meeting of the Satellite Division of The Institute of Navigation_ , 09 2017.
  * [17] P. Groves, _Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems, Second Edition_. Artech House, 2013.
  * [18] W. Wen, Y. C. Kan, and L.-T. Hsu, “Performance comparison of GNSS/INS integrations based on EKF and factor graph optimization,” in _Proc. of 32th Int. Technical Meeting of the Satellite Division of The Institute of Navigation_ , 09 2019, pp. 3019–3032.
  * [19] W. Wen, X. Bai, Y. C. Kan, and L.-T. Hsu, “Tightly coupled GNSS/INS integration via factor graph and aided by fish-eye camera,” _IEEE Trans. on Vehicular Technology_ , vol. 68, no. 11, pp. 10 651–10 662, 2019.
  * [20] W. Wen and L.-T. Hsu, “Towards robust GNSS positioning and real-time kinematic using factor graph optimization,” in _2021 IEEE Int. Conf. on Robotics and Automation (ICRA)_ , 2021, pp. 5884–5890.
  * [21] X. Bai, W. Wen, and L.-T. Hsu, “Time-correlated window-carrier-phase-aided GNSS positioning using factor graph optimization for urban positioning,” _IEEE Trans. on Aerospace and Electronic Systems_ , vol. 58, no. 4, pp. 3370–3384, 2022.
  * [22] T. Suzuki, “GNSS Odometry: Precise trajectory estimation based on carrier phase cycle slip estimation,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 3, pp. 7319–7326, 2022.
  * [23] B. Congram and T. D. Barfoot, “Field testing and evaluation of single-receiver gps odometry for use in robotic navigation,” _Field Robotics_ , vol. 2, pp. 1849–1873, 2022.
  * [24] T. Pfeifer, P. Weissig, S. Lange, and P. Protzel, “Robust factor graph optimization - a comparison for sensor fusion applications,” in _2016 IEEE 21st Int. Conf. on Emerging Technologies and Factory Automation (ETFA)_ , 2016, pp. 1–4.
  * [25] R.M.Watson and J. N. Gross, “Robust Navigation In GNSS Degraded Environment Using Graph Optimization.” 30th Int. Tech. Meeting of the Satellite Division of The Institute of Navigation, Sep 2017, pp. 2906–2918.
  * [26] T. Suzuki, “1st place winner of the smartphone decimeter challenge: Two-step optimization of velocity and position using smartphone’s carrier phase observations,” in _Proc. of the 35th Int. Technical Meeting of the Satellite Division of The Institute of Navigation_ , 09 2022, pp. 2276–2286.
  * [27] T. Pfeifer and P. Protzel, “Incrementally learned mixture models for GNSS localization,” in _2019 IEEE Intelligent Vehicles Symposium (IV)_ , 2019, pp. 1131–1138.
  * [28] W. Wen, G. Zhang, and L.-T. Hsu, “GNSS outlier mitigation via graduated non-convexity factor graph optimization,” _IEEE Trans. on Vehicular Technology_ , vol. 71, no. 1, pp. 297–310, 2022.
  * [29] K. Koide, J. Miura, and E. Menegatti, “A portable three-dimensional lidar-based system for long-term and wide-area people behavior measurement,” _Int. Journal of Advanced Robotic Systems_ , vol. 16, no. 2, p. 1729881419841532, 2019.
  * [30] J. Zhang, W. Wen, H. Feng, X. Chen, and L.-T. Hsu, “Continuous GNSS-RTK aided by LiDAR/Inertial odometry with intelligent GNSS selection in urban canyons,” in _Proc. of 34th Int. Technical Meeting of the Satellite Division of The Institute of Navigation_ , 09 2021, pp. 4198–4207.
  * [31] Z. Gong, P. Liu, F. Wen, R. Ying, X. Ji, R. Miao, and W. Xue, “Graph-based adaptive fusion of GNSS and VIO under intermittent GNSS-degraded environment,” _IEEE Trans. on Instrumentation and Measurement_ , vol. 70, pp. 1–16, 2021.
  * [32] S. Cao, X. Lu, and S. Shen, “GVINS: tightly coupled GNSS–Visual–Inertial fusion for smooth and consistent state estimation,” _IEEE Trans. on Robotics_ , vol. 38, no. 4, pp. 2004–2021, 2022\.
  * [33] W. Wen, X. Bai, and L.-T. Hsu, “3D vision aided GNSS real-time kinematic positioning for autonomous systems in urban canyons,” _NAVIGATION: Journal of the Institute of Navigation_ , vol. 70, no. 3, 2023.
  * [34] Y. Wang, W. Song, Y. Lou, Y. Zhang, F. Huang, Z. Tu, and Q. Liang, “Rail vehicle localization and mapping with LiDAR-vision-inertial-GNSS fusion,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 4, pp. 9818–9825, 2022.
  * [35] D. Wisth, M. Camurri, S. Das, and M. Fallon, “Unified multi-modal landmark tracking for tightly coupled LiDAR-Visual-Inertial odometry,” _IEEE Robotics and Automation Letters_ , vol. 6, no. 2, pp. 1004–1011, 2021.
  * [36] W. Shao, S. Vijayarangan, C. Li, and G. Kantor, “Stereo Visual Inertial LiDAR simultaneous localization and mapping,” in _2019 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS)_ , 2019, pp. 370–377.
  * [37] J. Nubert, S. Khattak, and M. Hutter, “Graph-based multi-sensor fusion for consistent localization of autonomous construction robots,” in _2022 Int. Conf. on Robotics and Automation (ICRA)_ , 2022, pp. 10 048–10 054.
  * [38] S. Zhao, H. Zhang, P. Wang, L. Nogueira, and S. Scherer, “Super Odometry: IMU-centric LiDAR-Visual-Inertial estimator for challenging environments,” in _2021 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS)_ , 2021, pp. 8729–8736.
  * [39] G. Cioffi and D. Scaramuzza, “Tightly-coupled fusion of global positional measurements in optimization-based visual-inertial odometry,” in _2020 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS)_ , 2020, pp. 5089–5095.
  * [40] S. Boche, X. Zuo, S. Schaefer, and S. Leutenegger, “Visual-inertial SLAM with tightly-coupled dropout-tolerant GPS fusion,” in _2022 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS)_ , 2022, pp. 7020–7027.
  * [41] C. Hu, S. Zhu, Y. Liang, and W. Song, “Tightly-coupled visual-inertial-pressure fusion using forward and backward imu preintegration,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 3, pp. 6790–6797, 2022.
  * [42] P. Furgale, T. D. Barfoot, and G. Sibley, “Continuous-time batch estimation using temporal basis functions,” in _2012 IEEE Int. Conf. on Robotics and Automation_ , 2012, pp. 2088–2095.
  * [43] D. Hug, P. Bänninger, I. Alzugaray, and M. Chli, “Continuous-time stereo-inertial odometry,” _IEEE Robotics and Automation Letters_ , vol. 7, no. 3, pp. 6455–6462, 2022.
  * [44] T. D. Barfoot, C. H. Tong, and S. Särkkä, “Batch continuous-time trajectory estimation as exactly sparse Gaussian process regression,” in _Robotics: Science and Systems_ , 2014.
  * [45] M. Mukadam, J. Dong, F. Dellaert, and B. Boots, “STEAP: simultaneous trajectory estimation and planning,” _Autonomous Robots_ , vol. 43, 02 2019\.
  * [46] X. Yan, V. Indelman, and B. Boots, “Incremental sparse GP regression for continuous-time trajectory estimation and mapping,” _Robotics and Autonomous Systems_ , vol. 87, pp. 120–132, 2017.
  * [47] J. Dong, J. G. Burnham, B. Boots, G. C. Rains, and F. Dellaert, “4D Crop Monitoring: Spatio-temporal reconstruction for agriculture,” _2017 IEEE Int. Conf. on Robotics and Automation (ICRA)_ , pp. 3878–3885, 2017.
  * [48] M. Warren, M. Greeff, B. Patel, J. Collier, A. P. Schoellig, and T. D. Barfoot, “There’s no place like home: Visual teach and repeat for emergency return of multirotor uavs during gps failure,” _IEEE Robotics and Automation Letters_ , vol. 4, no. 1, pp. 161–168, 2019.
  * [49] F. Dellaert and M. Kaess, “Factor graphs for robot perception,” _Foundations and Trends® in Robotics_ , vol. 6, no. 1-2, pp. 1–139, 2017\.
  * [50] J. Subirana, J. Zornoza, M. Hernández-Pajares, E. S. Agency, and K. Fletcher, _GNSS Data Processing_ , ser. ESA TM. ESA Communications, 2013, no. Bd. 1. [Online]. Available: https://books.google.de/books?id=RO8xngEACAAJ
  * [51] T. D. Barfoot, _State Estimation for Robotics_ , 1st ed. USA: Cambridge University Press, 2017.
  * [52] S. Anderson and T. D. Barfoot, “Full STEAM ahead: Exactly sparse Gaussian process regression for batch continuous-time trajectory estimation on SE(3),” in _2015 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS)_ , 2015, pp. 157–164.
  * [53] J. N. Wong, D. J. Yoon, A. P. Schoellig, and T. D. Barfoot, “A data-driven motion prior for continuous-time trajectory estimation on SE(3),” _IEEE Robotics and Automation Letters_ , vol. 5, no. 2, pp. 1429–1436, 2020\.
  * [54] T. D. Barfoot and P. T. Furgale, “Associating uncertainty with three-dimensional poses for use in estimation problems,” _IEEE Trans. on Robotics_ , vol. 30, no. 3, pp. 679–693, 2014.
  * [55] J. Dong, M. Mukadam, B. Boots, and F. Dellaert, “Sparse Gaussian processes on matrix lie groups: A unified framework for optimizing continuous-time trajectories,” in _2018 IEEE Int. Conf. on Robotics and Automation (ICRA)_ , 2018, pp. 6497–6504.
  * [56] J.-J. Gehrt, M. Nitsch, D. Abel, and R. Zweigel, “High accuracy navigation filter with dual antenna enabling double-differencing with dual-constellation,” in _32nd Int. Technical Meeting of the Satellite Division of the Institute of Navigation_ , 10 2019, pp. 2186–2197.
  * [57] T. Shan and B. Englot, “LeGO-LOAM: Lightweight and ground-optimized LiDAR odometry and mapping on variable terrain,” in _2018 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS)_ , 2018, pp. 4758–4765.
  * [58] T. Lupton and S. Sukkarieh, “Visual-Inertial-aided navigation for high-dynamic motion in built environments without initial conditions,” _IEEE Trans. on Robotics_ , vol. 28, no. 1, pp. 61–76, 2012.
  * [59] C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, “On-manifold preintegration for real-time visual-inertial odometry,” _IEEE Trans. on Robotics_ , vol. PP, 08 2016.
  * [60] M. Agrawal, “A lie algebraic approach for consistent pose registration for general euclidean motion,” in _2006 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems_ , 2006, pp. 1891–1897.
  * [61] J. T. Barron, “A general and adaptive robust loss function,” in _2019 IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR)_ , 2019, pp. 4326–4334.
  * [62] K. MacTavish and T. D. Barfoot, “At all costs: A comparison of robust cost functions for camera correspondence outliers,” in _2015 12th Conference on Computer and Robot Vision_ , 2015, pp. 62–69.
  * [63] S. W. Anderson, “Batch continuous-time trajectory estimation,” PhD thesis, University of Toronto, 4925 Dufferin Street, Toronto, ON M3H 5T6 CA, June 2017, available at https://hdl.handle.net/1807/77974.
  * [64] A. Ranganathan, M. Kaess, and F. Dellaert, “Fast 3D pose estimation with out-of-sequence measurements,” in _2007 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS)_ , 2007, pp. 2486–2493.
  * [65] M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. Leonard, and F. Dellaert, “iSAM2: Incremental smoothing and mapping with fluid relinearization and incremental variable reordering,” in _2011 IEEE Int. Conf. on Robotics and Automation_ , 2011, pp. 3281–3288.
  * [66] D. Wisth, M. Camurri, and M. Fallon, “Vilens: Visual, inertial, lidar, and leg odometry for all-terrain legged robots,” _IEEE Trans. on Robotics_ , vol. 39, no. 1, pp. 309–326, 2023.
  * [67] D. J. Yoon, K. Burnett, J. Laconte, Y. Chen, H. Vhavle, S. Kammel, J. Reuther, and T. D. Barfoot, “Need for speed: Fast correspondence-free lidar odometry using doppler velocity,” in _2023 IEEE/RSJ Int. Conf. on Intelligent Robots and Systems (IROS)_ , 2023.