\section{INTRODUCTION}
Safe and reliable autonomous driving operations in urban areas require accurate and consistent vehicle localization, that infers a smooth trajectory estimate and forms a basis for planning and control functions. In general, observations from Global Navigation Satellite Systems (GNSS), which enable global and accurate positioning, are used in the autonomous vehicles in large-scale environments.
However, the performance and positioning reliability of the GNSS are highly degenerated while the vehicle passes tunnels or urban canyons, where the loss of GNSS signal is expected and even more, the error dynamics of the GNSS observations grows increasingly complex due to multipath effects and None-Line-Of-Sight (NLOS) receptions. 
In contrast, many other localization methods incorporate local sensors into the vehicle, i.e. LiDARs and cameras that can be categorized as map-based or Simultaneous Localization And Mapping (SLAM) based. In these methods, landmarks from the sensor frames are extracted and associated to propose either frame-to-map global pose recovery or frame-to-frame local motion increments. As a high-quality map is not available in many areas, SLAM-based approaches using local sensors can achieve satisfactory localization quality if the environment can be well structured and sufficient loop-closure constraints can be detected for drift elimination. Still, these conditions cannot be fulfilled in large-scale environments \cite{slam_largescale}. \\
The most common way to ensure consistent urban localization is the fusion of different sensors. To do so, factor graph optimization (FGO) presents a smoothing-based state estimation framework for flexible sensor fusion. The graph provides a factorization of priors over the states and sensor measurement likelihoods \cite{factor_graph_review}. By default, the state estimation using FGO is accessible only after each optimization epoch, which is triggered whenever a new observation arrives. This emphasizes that most single-graph FGO methods are centered at one main sensor that schedules the optimization process. To fuse more sensor modalities, the measurements of other sensors must usually be synchronized with the main sensor.\\ 
In our previous work \cite{fgp_zhang}, we proposed a continuous-time FGO for the integration of GNSS and inertial measurements, where the optimization is triggered deterministically at a rate of 10Hz. If the GNSS measurements are delayed or cannot be synchronized to an existing state, the GNSS factor can still be evaluated by querying the state at the measurement time using a Gaussian Process (GP) motion prior. \\
In this work, we extend our previous investigation into a novel time-centric multi-sensor fusion approach with deterministic FGO procedure, which addresses the robust localization problem in large-scale environments. Compared to our previous work, we represent the system dynamics in the GP motion prior with a second-order White-Noise-On-Jerk model (WNOJ), as proposed in \cite{WNOJ}. Besides pseudorange and deltarange, the time-differenced carrier phase (TDCP) is also studied. To show the advantage of the time-centric multi-sensor fusion, we adapt the LiDAR odometry from the work \cite{liosam} and integrate the relative motion increments between two successive LiDAR frames. The proposed method in different settings is evaluated with data from a real-world measurement campaign in Aachen city. Our results show a consistent trajectory estimation with the multi-sensor fusion and improved accuracy, especially in the areas where the GNSS observations are not available. Based on our experiments, we present a comprehensive discussion of the effects on measurements and GP motion priors. For the benefit of research, we release our code and raw dataset from a 15km measurement campaign in Aachen city with different urban scenarios\footnotemark[1].
\footnotetext[1]{https://github.com/rwth-irt/onlineFGO.git}
\section{Related Work}
\subsection{FGO for GNSS-based Vehicle Localization}
Due to the limitations of GNSS standalone solutions in urban areas, a variety of FGO-based methods were proposed to enhance the GNSS-based vehicle localization accuracy.
It was shown in \cite{fgp_zhang} and \cite{ekf_fgo_compare} that the FGO generally outperforms the Kalman filter thanks to its smoothing effect. In \cite{hsu_rtk_fgo}, the double-differenced pseudorange (DDPR) and highly accurate carrier phase measurement (DDCP) are integrated in the FGO framework
and show a positive impact on performance. This work was extended by the authors to efficiently model the carrier phase constraints between multiple satellite measurement epochs inside a time window \cite{hsu_rtk2_fgo}. In \cite{taro_gnss_odom}, the time-differenced carrier phase (TDCP) is integrated with a factorized cycle slip detection, which can achieve highly accurate localization. Compared to the DDCP, which requires the GNSS measurements from a local base station, the TDCP has excellent availability under the condition that the GNSS receiver can track the satellites well. \\
Another research domain investigates the fusion of further sensor modalities into the FGO, such as LiDARs or cameras alongside the GNSS. By matching the landmarks of each sensor frame to the successive frame or the keyframe from a local model, relative motion increments can be calculated and integrated between pose constraints, as implemented in \cite{liosam, gnss_lidar_hsu,gvins,Geneva2017PPNIV}. The accuracy of such methods can degrade dramatically once the landmark extraction degenerates or no sufficient global constraints, such as GNSS observations or loop-closures, are available, as discussed in \cite{slam_largescale}.
\subsection{FGO for Multi-Sensor Fusion}
Because the fusion of GNSS with an extra sensor may still suffer from performance degeneration in complex scenarios, integrating many sensor modalities (more than two) into the FGO has recently drawn significant attention. \cite{gnss_lidar_hsu} proposes a loosely-coupled fusion of the synchronized LiDAR-Inertial-Odometry and GNSS positions in the FGO that improves the localization performance in deep urban areas. In \cite{liosam}, the GNSS positioning is fused alongside the LiDAR loop-closures and odometry factors in the FGO to propose drift-free LiDAR odometry, which is further used as a global reference in the IMU pre-integration \cite{imu_pre} for high frequency state estimation. The work \cite{gvins} presents a camera-centric sensor fusion approach similar to \cite{gnss_vio_adaptive} to obtain a smooth and consistent state estimate. The GNSS pseudoranges and deltaranges with IMU state propagation are fused to the visual-odometry in a tightly-coupled fashion. Another method employs two factor graphs implemented in parallel with a switching mechanism to fuse IMU, GNSS and LiDAR observations \cite{multisensor_construction}. When the GNSS antenna loses its signal, the secondary LiDAR-centric graph is activated to achieve consistent state estimation. Other works without the GNSS follow similar ideas to fuse the sensor modalities after careful sensor synchronization or with multiple graphs \cite{superodom, conti_vslam, lidar_vio_inertial_any, vio_lidar_withorder}.
\subsection{Continuous-Time Trajectory Representation}
One essential requirement for flexible graph-based multi-sensor fusion is the ability to query states associated with the observations within the iterative optimization process. This requirement can be met if the states are represented in continuous-time.
One confident approach utilizes Gaussian process (GP) regression  by assuming that system dynamics follow a linear time-varying stochastic differential equation (LTV-SDE) \cite{Barfoot2014BatchCT}. The system dynamics are normally modeled with a White-Noise-On-Acceleration (WNOA) model. This method using a WNOA assumption was validated in \cite{STEAP, gp-dong, Dong20174DCM}. Recently, the same author proposed an improved system dynamics model in \cite{WNOJ}, which assumes a White-Noise-On-Jerk (WNOJ) model. Because WNOJ presents the system dynamics in a second-order fashion, the author shows that WNOJ can model the vehicle dynamics more accurately than WNOA.

Inspired by the related work discussed above, we address multi-sensor fusion for vehicle localization in large-scale areas. Each sensor modality represents the vehicle's state or motion increments and is integrated into a single graph, where the synchronization between states and observations is not required. Our arguments are:
1. the fusion of local motion information from, e.g., LiDAR can be beneficial both in open-sky and GNSS-denied environments with consistent measurement noise models. 2. the continuous-time WNOJ motion prior is well suited to represent the dynamics that governs the system and enables more flexible sensor fusion. 

\section{Methodology}
\subsection{State-Space and Frames}
We define the state vector in the body-frame $\myFrameVec{b}{}{}$ located at the center of the IMU as $\myFrameVec{x}{}{}=[\myFrameVec{p}{b}{e}, \myFrameVec{v}{b}{e}, \myFrameVec{R}{b}{e}, \myFrameVec{b}{a}{}, \myFrameVec{b}{g}{}, \myFrameVec{c}{r}{}, (\myFrameVec{\omega}{b}{}), (\myFrameVec{s}{\mathrm{cyc}}{})]^T$, where the pose $(\myFrameVec{p}{b}{e}, ~\myFrameVec{R}{b}{e})$ and the velocity $\myFrameVec{v}{b}{e}$ are represented in the global Earth-Centered, Earth-Fixed (ECEF) frame $\myFrameVec{e}{}{}$. The vectors $\myFrameVec{b}{a}{}$ and $\myFrameVec{b}{g}{}$ denote the 3-D biases of accelerometer and gyroscope respectively. The 2-D vector $\myFrameVec{c}{r}{}$ represents the GNSS receiver clock bias and drift. If the GP-WNOJ/WNOA motion prior is used, the state vector is extended with the rotation rate $\myFrameVec{\omega}{b}{}$ in body-frame. For the integration of TDCP observations, the presence of cycle slips is estimated as a $N$-D vector $\myFrameVec{s}{\mathrm{cyc}}{}$ where $N$ is the number of satellites. \\
The fusion of the LiDAR measurements as local odometry requires a state-space representation in a local world frame $\myFrameVec{w}{}{}$ where the $z$-axis is gravity-aligned. We achieve this by querying the optimized state $\myFrameVec{x}{\mathrm{anc}}{e}=(\myFrameVec{p}{\mathrm{anc}}{e}, \myFrameVec{R}{\mathrm{anc}}{e})$ and using it as an anchor pose on the LiDAR-odometry initialization that defines the transformation between the global system pose in $\myFrameVec{e}{}{}$ frame and local LiDAR scan pose $\myFrameVec{x}{l}{w}=(\myFrameVec{p}{l}{w}, \myFrameVec{R}{l}{w})$ in $\myFrameVec{w}{}{}$ frame. For more details, see \ref{method: lidarodom}. 
\subsection{Measurement Models}
\subsubsection{GNSS Observation} \label{metho: gnssobs}
In this work, we integrate three GNSS observations: pseudorange (PR) $\rho$, deltarange (DR) $\nu$, and time-differenced carrier phase (TDCP) $\Delta\Phi$, to acquire global and relative constraints of system pose and velocity. All GNSS observations are pre-processed with the satellite-specific navigation messages and RTK correction data
 accouting for satellite clock or atmospheric delays, as in \cite{jan_filter_gnss}. The measurement models of $\rho$ and $\nu$ can be found in \cite{groves, jan_filter_gnss}. We implement the TDCP model proposed in \cite{taro_gnss_odom}, where the time difference between two pre-processed carrier phase observations $\Delta\myFrameScalar{\Phi}{t_2, t_1}{k}$ from the satellite $k$ is modeled as:
\begin{align}
\begin{split}
    \lambda\Delta\myFrameScalar{\Phi}{t_2, t_1}{k} &= \lambda [\myFrameScalar{\Phi}{t_2}{k} - \myFrameScalar{\Phi}{t_1}{k}] \\
    &\simeq \Delta\myFrameScalar{r}{t_2, t_1}{k} + \myFrameVec{u}{t_2}{k}\cdot\Delta\myFrameVec{p}{t_2}{t_1} + c\Delta\myFrameScalar{\delta}{t_2, t_1}{} + \\
    &+ \lambda(\myFrameScalar{B}{t_2}{k} - \myFrameScalar{B}{t_1}{k}) + \myFrameScalar{\Delta\epsilon}{t_2, t_1}{}
\end{split}
\end{align}
where the signal wavelength is denoted as $\lambda$. The geometric range difference between antenna position and the satellite $k$ at two timestamps $t_1$ and $t_2$ is given by $\Delta\myFrameScalar{r}{t_2, t_1}{k}$. $\myFrameVec{u}{t_2}{k}$ represents the direction vector between antenna and satellite at time $t_2$. The 3-D antenna relative position from $t_2$ to $t_1$ is denoted as $\Delta\myFrameVec{p}{t_2}{t_1}$. $c$ is the light speed and $\Delta\delta$ the receiver clock delay change. We use $\myFrameScalar{B}{t}{k}$ to represent the carrier phase ambiguity, which remains constant between successive measurement epochs if no cycle slip for the satellite has been detected. $\myFrameScalar{\Delta\epsilon}{t_2, t_1}{}$ is the measurement noise different between two timestamps.

\subsubsection{LiDAR Odometry} \label{method: lidarodom}
We adapt the feature extraction and matching methods from a feature-based LiDAR-odometry \cite{loam, liosam} to integrate the relative motion constraints between two laser scans into the proposed FGO. The undistorted raw points in each scan are clustered into planar and edge features $\mathbfcal{F}_t=\{\myFrameVec{F}{t}{e}, \myFrameVec{F}{t}{p}\}$ based on the smoothness metric shown in \cite{loam, legoloam}. In the matching process, all $k$ features in $\mathbfcal{F}_{t+1}$ of the current scan are associated with pose priors $\myFrameVec{p}{t+1,1:k}{w}$ and used to find the best transformation $\Delta\myFrameVec{T}{t, t+1}{w}$ to the last scan by solving an optimization problem that takes the distance between corresponding features in $\mathbfcal{F}_{t}$ as objective using the Gauss-Newton algorithm. We refer the reader to \cite{loam, legoloam} for more details. \\
In contrast to the work \cite{liosam}, we query the vehicle states at scan times from the FGO and integrate the transformation $\Delta\myFrameVec{T}{t, t+1}{w}$ as between pose constraints into the graph. After the graph optimization, we query the optimized states again using the GP motion prior and update the poses of the LiDAR keyframes in the $\myFrameVec{w}{}{}$ frame using the following transformation:
\begin{align}
    \myFrameVec{p}{l,t}{w} &= \myFrameVec{R}{n}{w}\myFrameVec{R}{anc, e}{n}(\myFrameVec{p}{l,t}{e} - \myFrameVec{p}{anc}{e}) \\
    \myFrameVec{R}{l,t}{w} &= \myFrameVec{R}{n}{w}\myFrameVec{R}{anc, e}{n}\myFrameVec{R}{l,t}{e}
\end{align}
where $\myFrameVec{p}{l,t}{e}$ and $\myFrameVec{R}{l,t}{e}$ are position and rotation of the LiDAR center in $\myFrameVec{e}{}{}$ frame at time $t$. The matrix $\myFrameVec{R}{anc, e}{n}$ is the rotation matrix of the anchor point from $\myFrameVec{e}{}{}$ frame to the East, North, Up (ENU) frame $\myFrameVec{n}{}{}$, which is calculated by the direction cosine matrix of the geodetic coordinates of the anchor point. The rotation $\myFrameVec{R}{n}{w}$ presents the yaw-angle offset from the $\myFrameVec{n}{}{}$ frame to $\myFrameVec{w}{}{}$ frame of the anchor point. In contrast to \cite{gvins}, the estimation of this offset is not necessary in our work because this orientation is given from the FGO.    

\subsection{Continuous-Time Trajectory Representation} 
Following the approach of Tang \textit{et al.} in \cite{WNOJ}, we represent the vehicle trajectory in continuous-time by formulating the system dynamics using a linear time-varying stochastic differential equation (LTV-SDE):
\begin{align}
    \label{eq: ltvsde}
    \begin{split}
        \myFrameVecDot{\gamma}{}{}(t) &= \myFrameVec{A}{}{}\myFrameVec{\gamma}{}{}(t) + \myFrameVec{B}{}{}\myFrameVec{u}{}{}(t) + \myFrameVec{F}{}{}\myFrameVec{w}{}{}(t) \\
        \myFrameVec{w}{}{}(t) &\sim \mathcal{GP}(0,~\myFrameVec{Q}{c}{}\cdot\delta(t-t^{'}))
    \end{split}
\end{align}
where the state is denoted by $\myFrameVec{\gamma}{}{}(t)$ as local state variables and $\myFrameVec{A}{}{}$ and $\myFrameVec{F}{}{}$ are the time-varying system matrices. The input vector $\myFrameVec{u}{}{}(t)$ is set to $\myFrameVec{0}{}{}$. The process noise $\myFrameVec{w}{}{}(t)$ is given as a zero-mean, GP with power spectral density matrix $\myFrameVec{Q}{c}{} \in \mathbb{R}^{6\times6}$. The Dirac delta function is denoted by $\delta(t)$. In discrete time, the dynamic matrix $\myFrameVec{A}{}{}$ is transformed into the system transition matrix $\myFrameVec{\Phi}{t_k}{}$. \\
A GP motion prior in $\myFrameVec{\gamma}{}{}(t)$ can be specified in the following way:
\begin{align}
\label{eq: gpprior}
    \begin{split}
         \myFrameVecDot{T}{}{}(t) &= \myFrameVec{\varpi}{}{}(t)^\land \myFrameVec{T}{}{}(t) \\
         \myFrameVecDot{\varpi}{}{}(t) &= \myFrameVec{w}{}{}(t)
    \end{split}
\end{align}
In \eqref{eq: gpprior}, the vehicle pose is represented as $\myFrameVec{T}{}{}(t) = \exp{(\myFrameVec{\xi}{}{}(t)^\land)}$ with local pose $\myFrameVec{\xi}{}{}(t) = [\myFrameVec{\rho}{}{}(t)^T, ~ \myFrameVec{\Phi}{}{}(t)^T]^T \in \mathbb{R}^{6}$, which is converted into $\mathfrak{se}(3)$ by applying the operator $(\cdot)^\land$ \cite{BarfootBook}. The operator $(\cdot)^\lor$ is the inverse of $(\cdot)^\land$. $\myFrameVec{\rho}{}{}(t)$ and $\myFrameVec{\Phi}{}{}(t)$ are the translational and rotational state vectors respectively, and $\myFrameVec{\varpi}{}{}(t)=[\myFrameVec{\nu}{}{}(t)^T,~\myFrameVec{\omega}{}{}(t)^T]^T \in \mathbb{R}^{6}$ represents the body-centric velocity. \\
The GP prior in \eqref{eq: gpprior} cannot be processed directly in \eqref{eq: ltvsde} because of the non-linearity. However, the authors of \cite{fullsteam} showed that this problem can be solved by transforming the global state $\myFrameVec{T}{}{}(t)$ into the local frame $\myFrameVec{\xi}{}{}(t)$ by  $\myFrameVec{\xi}{i}{}(t) = \ln(\myFrameVec{T}{}{}(t)\myFrameVec{T}{t_i}{-1})^\lor, ~ t_i \leq t  \leq t_{i+1}$.
This idea fulfills the requirement for the GP motion prior in our work. And thus, we retrieve the WNOJ motion model:
\begin{align}
    \begin{split}
        \myFrameVec{\gamma}{i}{}(t_i) &= [\myFrameVec{0}{}{}, ~\myFrameVec{\varpi}{}{}(t)^T, ~\myFrameVecDot{\varpi}{}{}(t)^T]^T \\
        \myFrameVec{\gamma}{i}{}(t_{i+1}) &= \left[\begin{matrix}
                                                    \ln(\myFrameVec{T}{i+i,i}{})^\lor\\
                                                    \mathbfcal{J}_{i+1}^{-1}\myFrameVec{\varpi}{i+1}{} \\
                                                    -\frac{1}{2}(\mathbfcal{J}_{i+1}^{-1}\myFrameVec{\varpi}{i+1}{})^\curlywedge\myFrameVec{\varpi}{i+1}{} + \mathbfcal{J}_{i+1}^{-1}\myFrameVecDot{\varpi}{i+1}{}
                                                    \end{matrix} \right]
    \end{split}
\end{align}
where the definition of the $(\cdot)^\curlywedge$ operator can be found in equation (35) in \cite{WNOJ}. $\mathbfcal{J}_{i+1}^{}$ is the left Jacobian in $\mathfrak{se}(3)$, which can be assumed as $\myFrameVec{1}{}{}$ over small intervals $\mathcal{I} = [i, i+1]$ \cite{gp-slam}. We refer the reader to \cite{WNOJ, BarfootBook} for details. \\
Compared to the WNOA model, querying the state at an arbitrary time $\tau \in \mathcal{I}$ is achieved by: 
\begin{align}
    \begin{split}
        \myFrameVec{T}{\tau}{} &= \exp\Big \{ [ \myFrameVec{\Lambda}{12}{}(\tau)\myFrameVec{\varpi}{i}{} + \myFrameVec{\Omega}{13}{}(\tau)\myFrameVecDot{\varpi}{i}{} + \myFrameVec{\Sigma}{11}{}(\tau)\ln(\myFrameVec{T}{i+i,i}{})^\lor +\\
                               & + \myFrameVec{\Omega}{12}{}(\tau)\mathbfcal{J}_{i+1}^{-1}\myFrameVec{\varpi}{i+1}{} + \\
                               & + \myFrameVec{\Omega}{13}{}(\tau)( -\frac{1}{2}(\mathbfcal{J}_{i+1}^{-1}\myFrameVec{\varpi}{i+1}{})^\curlywedge\myFrameVec{\varpi}{i+1}{} + \mathbfcal{J}_{i+1}^{-1}\myFrameVecDot{\varpi}{i+1}{})]^\land \Big\} \myFrameVec{T}{i}{}
    \end{split}
\end{align}
where the $\myFrameVec{\Lambda}{}{}$ and $\myFrameVec{\Omega}{}{}$ are interpolation matrices based on the system transition $\myFrameVec{\Phi}{}{}$ and the covariance matrix $\myFrameVec{Q}{}{}$, which are given in equation (9-11) in \cite{fgp_zhang}. \\
With the WNOJ model, the covariance matrix $\myFrameVec{Q}{i}{}(t)$ is given as
\begin{equation}
    \myFrameVec{Q}{i}{}(t) = \left[ \begin{matrix} 
                           \frac{1}{20}\Delta t_i^5\myFrameVec{Q}{c}{} & \frac{1}{8}\Delta t_i^4\myFrameVec{Q}{c}{} & \frac{1}{6}\Delta t_i^3\myFrameVec{Q}{c}{}\\
                           \frac{1}{8}\Delta t_i^4\myFrameVec{Q}{c}{}  & \frac{1}{3}\Delta t_i^3\myFrameVec{Q}{c}{} & \frac{1}{2}\Delta t_i^2\myFrameVec{Q}{c}{}\\
                           \frac{1}{6}\Delta t_i^3\myFrameVec{Q}{c}{}  & \frac{1}{2}\Delta t_i^2\myFrameVec{Q}{c}{} & \Delta t_i\myFrameVec{Q}{c}{}
                           \end{matrix}  \right]
\end{equation}
which scales the states in $\myFrameVec{\gamma}{i}{}(t)$ w.r.t. to the relative time based on the hyper-parameter $\myFrameVec{Q}{c}{}$.

\subsection{Factor Formulation}
Because of the page limitation, we only present the factor models which are not introduced in our previous work \cite{fgp_zhang}.
\subsubsection{Time-Differenced Carrier Phase Factor}
We formulate the TDCP factor similar to \cite{taro_gnss_loop}, but rather with a robust noise model using a Cauchy-loss M-estimator.
The error between the augmented and measured TDCPs between queried states $i$ and $i+1$ for satellite $k$ is given by:
\begin{equation}
\begin{split}
 \left\|\myFrameVec{e}{i+1,i}{\text{TDCP}}\right\|^2 = \| 
\Delta\myFrameScalar{r}{i+1, i}{k} + \myFrameVec{u}{i+1}{k}\cdot\Delta\myFrameVec{p}{i+1}{i} + \Delta t \myFrameScalarDot{\delta}{i+1, i}{k} + \\ 
 +\lambda(\myFrameScalar{B}{i+1}{k} - \myFrameScalar{B}{i}{k}) - \lambda\Delta\myFrameScalar{\Phi}{i+1, i}{k} \|_{\myFrameVec{\Sigma}{\text{TDCP}}{}}^2
\end{split}
\label{eq:err_tdcp}
\end{equation}
In the implementation, we utilize the receiver clock drift $c_d \in \myFrameVec{c}{r}{}$ multiplied by $\Delta t$ to represent the clock bias change. The measurement noise $\myFrameVec{\Sigma}{\text{TDCP}}{}$ is given by the receiver and is manually scaled with a hyper-parameter for robust optimization. Analogous to the GNSS factors in \cite{fgp_zhang}, the states are interpolated using GP motion priors once the GNSS observation is delayed or cannot be associated with the state within a very short time interval. 

\subsubsection{Cycle Slip Detection Factor}
The cycle slip detection factor between state $\myFrameVec{s}{cyc,i+1}{}$ and $\myFrameVec{s}{cyc,i}{}$ is modeled as follows:
\begin{equation}
\label{eq:err_amb}
    \left\|\myFrameVec{e}{i}{\text{B}}\right\|^2 = \left\| \myFrameScalar{B}{i+1}{k} - \myFrameScalar{B}{i}{k} \right\|_{\myFrameScalar{\Sigma}{\text{B}}{}}^2
\end{equation}
where the variance $\myFrameScalar{\Sigma}{B}{}$ is set to a small value, e.g. $0.01$, if the satellite is continuously tracked and modified to a very big value, if the satellite is lost or a cycle slip occurs. In this case, we also add a soft ambiguity lock prior factor with a very large variance to the state $\myFrameVec{s}{cyc}{}$ to avoid an ill-posed optimization problem.

\subsubsection{Between Pose Factor}
The between pose factor in $SE3$ can be represented as:
\begin{equation}
\label{eq:err_between}
    \left\|\myFrameVec{e}{i+1, i}{\text{BP}}\right\|^2 = \left\| \textrm{logmap}(\textrm{comp}(\myFrameVec{p}{i}{e}, \myFrameVec{p}{i+1}{e}), ~\Delta\myFrameVecTilde{T}{i, i+1}{e})
          \right\|_{\myFrameVec{\Sigma}{\text{BP}}{}}^2
\end{equation}
where the operator $\textrm{comp}(\cdot)$ denotes the calculation of the relative transformations between two poses in the Lie group. The error between the estimated relative transformation and the measured relative transformation $\Delta\myFrameVecTilde{T}{i, i+1}{e}$ from LiDAR-odometry is calculated using the logarithmic mapping $\textrm{logmap}(\cdot)$ proposed in \cite{theory_betweenpose}.

\subsubsection{Proposed Factor Graph}
The resulting factor graph from the proposed FGO is shown in Fig.\,\ref{fig:fg_proposed}. The states $\myFrameVec{x}{t}{}$ are deterministically created on the graph independent of any measurement. If a measurement cannot be associated with any state, between-states $\myFrameVecHat{x}{t-\tau}{}$, which are not present in the optimization lag, are queried for the error evaluation. To achieve flexible sensor fusion, the factors can be configured on demand.
The optimization problem can then be formulated as:
\begin{equation}
    \begin{split}
       &\myFrameVecHat{X}{}{} =\underset{\myFrameVec{x}{}{}}{\arg\min}\Big[
        \sum_{i=1}^{t}\sum_{s=1}^{k}\myFrameVecHat{e}{s,i}{\text{PRDR}} +
        \sum_{i=1}^{t}\left\|\myFrameVec{e}{i}{\text{IMU}}\right\|
        + \sum_{i=1}^{t'}\left\|\myFrameVec{e}{i}{\text{BP}}\right\|\\
        &+ \sum_{i=1}^{t''}(\myFrameVecHat{e}{s,i}{\text{TDCP}} + \left\|\myFrameVec{e}{i}{\text{B}}\right\|) + \sum_{i=1}^{t}\left\|\myFrameVec{e}{i}{\text{Clo}}\right\|
        +\sum_{i=1}^{t}\left\|\myFrameVec{e}{i}{\text{GP}}\right\| + \left\|\myFrameVec{e}{i}{0}\right\|_{\myFrameVec{\Sigma}{0}{}}^2 \Big]
    \end{split}
    \label{eq:fg_map}
\end{equation}

\begin{figure}[!ht]
    \centering
    \includegraphics[width=0.45\textwidth]{figures/fgo_v2_300dpi.png}
    \caption{Factor graph proposed in this work}
    \label{fig:fg_proposed}
\end{figure}

\subsection{System Overview}
In this work, we acquire IMU measurements at 100Hz and the GNSS observations at 10Hz without delay. The PPS signal from the GNSS receiver is utilized to calculate the time delay and synchronize LiDAR scans that are expected at 10Hz. The RTCMv3 correction messages are received from the local SAPOS\footnote{http://www.sapos.nrw.de} server via internet at about 1Hz.
After each optimization epoch, the IMU pre-integrator for a state propagation at 100Hz is reset with new optimized bias and the poses of LiDAR keyframes are corrected, if LiDAR-odometry is used.
We use the positioning and orientation estimation solution from a dual-antenna GNSS setup to initialize the state $\myFrameVec{x}{0}{}$. The FGO is solved with the Batch fixed-lag smoother, as proposed in \cite{fixedlagRanganathan}. \\
The proposed FGO and all related processing functionalities are implemented in C\texttt{++} within the open-source software framework Robot Operating System 2 (ROS2)\footnote{https://docs.ros.org/en/rolling/index.html} and GTSAM\footnote{https://gtsam.org/}. We adapt the LiDAR-odometry front-end from liosam\footnote{https://github.com/TixiaoShan/LIO-SAM}.

\begin{figure*}[!ht]
    \centering
    \includegraphics[width=1\textwidth]{figures/illu150dpi.png}
    \caption{Estimated trajectory and LiDAR local map plot. Green trajectory is with proposed FGO by integrating all measurements, the red is the reference from the GNSS receiver. In LiDAR maps, the middle points illustrate the LiDAR keyframe poses in local world frame. }
    \label{fig:fgo_trajectory}
\end{figure*}

\section{Experiment and Results}
\subsection{Experiment Setup and Dataset}
The measurement campaign was organized with a sensor suite that includes a Microstrain 3DM-GX5-25 IMU, a dual-antenna GNSS receiver NovAtel PwrPak7D and a Velodyne VLP-16 LiDAR on a test vehicle, as illustrated in Fig.\,\ref{fig:mea_hardware}.
The campaign contains a 15km test drive beginning from the Campus Melaten area and proceeding to the downtown area of Aachen. There urban scenarios can be found in the dataset: open-sky, sub/semi-urban and urban. In the experiments, we used a laptop with an Intel i9-9900K, 16 Cores at max. 4.7GHz and 64 GB memory. 

\begin{figure}[!ht]
    \centering
    \includegraphics[width=0.45\textwidth]{figures/MeasSetup120dpi.png}
    \caption{Measurement setup on top of the test vehicle.}
    \label{fig:mea_hardware}
\end{figure}


\subsection{Results and Discussion}
Fig.\,\ref{fig:fgo_trajectory} presents the estimated trajectory (green) and samples from the LiDAR's local maps. In open-sky and semi-urban areas, as shown in Fig.\,\ref{fig:fgo_trajectory}.f. a precise and accurate LiDAR local map can be constructed. As an online localization solution, the computation time of the optimization procedure takes $0.03s$ on average with a maximum of $0.9s$ on the test laptop with a fixed lag size of $3s$ for all measurements.

\subsubsection{General Error Metrics}
The general errors for different routes, where a RTK-fixed GNSS solution is available, are given in Tab.\,\ref{tab:errors}. We also plot the 2D position error in Fig.\,\ref{fig:2derror}. In the open-sky or sub-urban areas, accurate and precise trajectory estimation can be shown from the results. The relative pose constraints both from TDCP and LiDAR-odometry are able to improve the estimation performance effectively. Once the GNSS observations are strongly corrupted in urban areas, the LiDAR-odometry can contribute in some scenarios as shown in Fig.\,\ref{fig:fgo_trajectory}.d, but trajectory discontinuity and light bias still cannot be avoided in some of these cases, see Fig.\,\ref{fig:fgo_trajectory}.e and g. Moreover, it can be observed that the contribution of the TDCP and LiDAR-odometry to the performance varies in different scenes. In open-sky areas, the TDCP contributes more to the accuracy while the LiDAR-odometry constraints the error in urban scenes effectively. We also note here, the ground-truth reference in urban areas may also be corrupted. 

\begin{table}[!ht]
\centering
\caption{General Position Errors: Route \#1: Total route (0-2476s), \#2: Open-Sky (0-160s), \#3: Urban with re-initialization of the FGO (701-928s)} 
\label{tab:errors}
\begin{tabular}{|c|c|ccc|ccc|}
\hline
\multicolumn{1}{|l|}{\textbf{}}                                                   & \multicolumn{1}{l|}{\textbf{}} & \multicolumn{3}{c|}{\textbf{2D {[}m{]}}}                                                & \multicolumn{3}{c|}{\textbf{3D {[}m{]}}}                                                \\ \hline
\textbf{Settings}                                                                 & \textbf{R.}                    & \multicolumn{1}{c|}{\textbf{Max.}} & \multicolumn{1}{c|}{\textbf{Min.}} & \textbf{Mea.} & \multicolumn{1}{c|}{\textbf{Max.}} & \multicolumn{1}{c|}{\textbf{Min.}} & \textbf{Mea.} \\ \hline
\multirow{3}{*}{\begin{tabular}[c]{@{}c@{}}GNSS-all\\ w. \\ LiDAR\end{tabular}}   & \#1                            & \multicolumn{1}{c|}{2.55}             & \multicolumn{1}{c|}{0.004}             & 0.99   & \multicolumn{1}{c|}{2.75}             & \multicolumn{1}{c|}{0.04}             &   1.21  \\ \cline{2-8} 
                                                                                  & \#2                            & \multicolumn{1}{c|}{0.53}             & \multicolumn{1}{c|}{0.03}             & 0.31           & \multicolumn{1}{c|}{0.58}             & \multicolumn{1}{c|}{0.04}             & 0.34            \\ \cline{2-8} 
                                                                                  & \#3                            & \multicolumn{1}{c|}{1.17}             & \multicolumn{1}{c|}{0.03}             & 0.42            & \multicolumn{1}{c|}{1.24}             & \multicolumn{1}{c|}{0.04}             & 0.51             \\ \hline
\multirow{3}{*}{\begin{tabular}[c]{@{}c@{}}PRDR\\ w. \\ LiDAR\end{tabular}}       & \#1                            & \multicolumn{1}{c|}{2.65}              & \multicolumn{1}{c|}{0.004}              &      \multicolumn{1}{c|}{0.99}          & \multicolumn{1}{c|}{2.70}              & \multicolumn{1}{c|}{0.05}              &      1.33         \\ \cline{2-8} 
                                                                                  & \#2                            & \multicolumn{1}{c|}{0.85}              & \multicolumn{1}{c|}{0.04}              &    0.40          & \multicolumn{1}{c|}{1.03}              & \multicolumn{1}{c|}{0.05}              &      0.45         \\ \cline{2-8} 
                                                                                  & \#3                            & \multicolumn{1}{c|}{1.34}              & \multicolumn{1}{c|}{0.003}              &    0.66           & \multicolumn{1}{c|}{1.35}             & \multicolumn{1}{c|}{0.003}             & 1.13              \\ \hline
\multirow{3}{*}{\begin{tabular}[c]{@{}c@{}}GNSS-all\\ w/o. \\ LiDAR\end{tabular}} & \#1                            & \multicolumn{1}{c|}{2.83}             & \multicolumn{1}{c|}{0.02}             & 0.83             & \multicolumn{1}{c|}{2.87}             & \multicolumn{1}{c|}{0.12}             & 1.04    \\ \cline{2-8} 
                                                                                  & \#2                            & \multicolumn{1}{c|}{0.82}             & \multicolumn{1}{c|}{0.04}             & 0.50             & \multicolumn{1}{c|}{1.00}             & \multicolumn{1}{c|}{0.12}             & 0.59            \\ \cline{2-8} 
                                                                                  & \#3                            & \multicolumn{1}{c|}{1.23}             & \multicolumn{1}{c|}{0.03}             & 0.58             & \multicolumn{1}{c|}{1.32}             & \multicolumn{1}{c|}{0.05}             & 0.64            \\ \hline
\multirow{3}{*}{\begin{tabular}[c]{@{}c@{}}PRDR \\ only\end{tabular}}             & \#1                            & \multicolumn{1}{c|}{2.99}              & \multicolumn{1}{c|}{0.01}              &    1.05          & \multicolumn{1}{c|}{3.01}              & \multicolumn{1}{c|}{0.02}              &      1.34         \\ \cline{2-8} 
                                                                                  & \#2                            & \multicolumn{1}{c|}{0.76}              & \multicolumn{1}{c|}{0.02}              &     0.44          & \multicolumn{1}{c|}{0.94}              & \multicolumn{1}{c|}{0.08}              &    0.53           \\ \cline{2-8} 
                                                                                  & \#3                            & \multicolumn{1}{c|}{1.27}              & \multicolumn{1}{c|}{0.1}              &   1.93            & \multicolumn{1}{c|}{1.64}              & \multicolumn{1}{c|}{0.08}              &     0.73       \\ \hline                                                                              
\end{tabular}
\end{table}

\begin{figure}[!ht]
    \centering
    \includegraphics[width=0.48\textwidth]{figures/2dErr300dpi.png}
    \caption{2D Position Error.}
    \label{fig:2derror}
\end{figure}
\subsubsection{Evaluation of GP Motion Priors}
We have shown in our previous work \cite{fgp_zhang} that the integration of a GP motion prior with the WNOA model can constrain the drift introduced by corrupted measurements efficiently. In this work, we follow the idea of \cite{WNOJ} and implement the WNOJ model for the GP motion prior. We have experimented with both motion models in an open-sky area only using a PRDR factor. Both models are tuned with the predetermined optimal hyper-parameter $\myFrameVec{Q}{c}{}$ \footnotemark[1]. As shown in Fig.\,\ref{fig:eva_gp_tdcp}, the GP-WNOJ prior is able to capture the turning motion while there is a large bias for the GP-WNOA. Thus, we provide more supporting evidence for the claim in \cite{WNOJ} that a second order WNOJ model is better suited to represent a system with stronger dynamics.
\footnotetext[1]{We set $\myFrameVec{Q}{c}{}$ as a diagonal matrix with identical entries $\myFrameScalar{Q}{c}{}$. For WNOA, $\myFrameScalar{Q}{c}{}=0.5$. For WNOJ, $\myFrameScalar{Q}{c}{\mathrm{Prior}}=1000$, and $\myFrameScalar{Q}{c}{\mathrm{Interpolator}}=100.$.} Fig.\,\ref{fig:eva_gp_tunnel} shows the trajectories resulting from configurations with different motion priors while crossing a tunnel, where the impact of the between states constraints from a GP prior model can be shown. 
\begin{figure}[!ht]
    \centering
    \includegraphics[width=0.4\textwidth]{figures/evaGP150dpi.png}
    \caption{Evaluation of the model capacity between WNOA / WNOJ and TDCP only.}
    \label{fig:eva_gp_tdcp}
\end{figure}
\begin{figure}[!ht]
    \centering
    \includegraphics[width=0.4\textwidth]{figures/evaGPBridge150dpi.png}
    \caption{Evaluation of the trajectory smoothness while crossing a tunnel.}
    \label{fig:eva_gp_tunnel}
\end{figure}
\subsubsection{Evaluation of TDCP observations}
We have evaluated the impact of the TDCP factors used in conjunction with IMU, GP-WNOJ and PRDR factors in an open-sky area, as shown with route \#2 in Tab.\,\ref{tab:errors}. The experiments show that integrating only TDCP factors from more than 3 satellites alongside PRDR factors can improve the accuracy, if the noise models of the TDCP factors are set properly. Because the TDCP factors only constrain the relative vehicle poses, it is not sufficient to calibrate the error after the initialization and IMU bias estimation, as plotted in Fig.\,\ref{fig:eva_gp_tdcp}. This shows supplementary results to the work \cite{taro_gnss_odom}. Also, the integration of TDCP does not contribute to the trajectory consistency in a meaningful way if the GNSS measurements are strongly corrupted or the amount of tracked satellites is not sufficient, which was not discussed in \cite{taro_gnss_odom}. We show this with the route \#3 in Tab.\,\ref{tab:errors}.

\subsubsection{Evaluation of LiDAR Odometry}
Although we have shown that the integration of LiDAR-odometry can improve the accuracy and consistency of the trajectory with corrupted GNSS measurements, as illustrated in Fig.\,\ref{fig:fgo_trajectory}.e, c, and f, it can still degenerate in some scenarios. If the urban scene is less structureable as shown in Fig.\,\ref{fig:lidar_degen}.a, or the vehicle motion is near zero-velocity as in Fig.\,\ref{fig:lidar_degen}.b, where the orientation is not absolutely observable, the LiDAR-odometry becomes less accurate. Again, if the noise models of other measurements, e.g., GNSS, are strongly inconsistent, the integration of LiDAR-odometry cannot contribute as expected.
\begin{figure}[!ht]
    \centering
    \includegraphics[width=0.5\textwidth]{figures/lidar_degen_150dpi.png}
    \caption{Degeneration of LiDAR-odometry at: a. less structureable area; b. near zero-velocity vehicle motion.}
    \label{fig:lidar_degen}
\end{figure}

\section{Conclusion and Future Work}
In this work, we propose a novel, continuous-time factor graph optimization with time-centric multi-sensor integration to achieve robust vehicle localization in challenging urban environments. The IMU and GNSS observations are factored into the graph. A Gaussian process motion prior with the surrogate model WNOJ is employed to represent the trajectory in continuous-time, enabling state queries at arbitrary times. Therefore, states need not be aligned with measurements to trigger the optimization process. This empowers a scalable and flexible sensor fusion that produces improved state estimates. From our driving experiments in Aachen city, the improvement of both accuracy and robustness of the vehicle localization can be shown when the GNSS observations are corrupted or not available. We have confirmed the hypothesis that the second order GP-WNOJ motion model outperforms GP-WNOA when sensible hyper-parameters are available $\myFrameVec{Q}{c}{}$. Discussions regarding the degeneration of LiDAR-odometry in urban areas and hyper-parameter settings were proposed. Our framework is not limited to other sensors. Based on the results, we indicate the future work in following directions: 1. integration of loop-closure constraints to improve the long-term performance; 2. extending the FGO for hybrid optimization in discrete-time space, e.g., cycle slip detection; 3. online hyper-parameter tuning using the residuals for better model consistency.

\newpage
                                 
                                 
                                 
                                 
                                 









\bibliographystyle{IEEETran.bst}
