ZUPT Aided GNSS Factor Graph with Inertial Navigation Integration for Wheeled Robots

by   Cagri Kilic, et al.

In this work, we demonstrate the importance of zero velocity information for global navigation satellite system (GNSS) based navigation. The effectiveness of using the zero velocity information with zero velocity update (ZUPT) for inertial navigation applications have been shown in the literature. Here we leverage this information and add it as a position constraint in a GNSS factor graph. We also compare its performance to a GNSS/inertial navigation system (INS) coupled factor graph. We tested our ZUPT aided factor graph method on three datasets and compared it with the GNSS-only factor graph.



page 5


Zero-Velocity Detection -- A Bayesian Approach to Adaptive Thresholding

A Bayesian zero-velocity detector for foot-mounted inertial navigation s...

Exploring the Accuracy Potential of IMU Preintegration in Factor Graph Optimization

Inertial measurement unit (IMU) preintegration is widely used in factor ...

Adaptive Step Size Learning with Applications to Velocity Aided Inertial Navigation System

Autonomous underwater vehicles (AUV) are commonly used in many underwate...

IMU Data Processing For Inertial Aided Navigation: A Recurrent Neural Network Based Approach

In this work, we propose a novel method for performing inertial aided na...

Sensor Fusion for Magneto-Inductive Navigation

Magneto-inductive navigation is an inexpensive and easily deployable sol...

Robust Data-Driven Zero-Velocity Detection for Foot-Mounted Inertial Navigation

We present two novel techniques for detecting zero-velocity events to im...

FootSLAM meets Adaptive Thresholding

Calibration of the zero-velocity detection threshold is an essential pre...
This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.


In this work, we demonstrate the importance of zero velocity information for global navigation satellite system (GNSS) based navigation. The effectiveness of using the zero velocity information with zero velocity update (ZUPT) for inertial navigation applications have been shown in the literature. Here we leverage this information and add it as a position constraint in a GNSS factor graph. We also compare its performance to a GNSS/ inertial navigation system (INS) coupled factor graph. We tested our ZUPT aided factor graph method on three datasets and compared it with the GNSS-only factor graph.

I Introduction

Localization is an integral component of any mobile robot system, which plays an important role in many core robotic capabilities like motion planning, obstacle avoidance, and mapping. One of the most common methods for calculating positioning information of the robots is using GNSS [enge1994global]. However, the availability of this system relies on the observation of multiple satellites [grovebook]

and is frequently unable to obtain a precise and robust state estimate in urban and forested areas due to environmental constraints (e.g., poor satellite geometry and multipath effects) 

[merry2019smartphone]. An inertial measurement unit (IMU) can be used in a standalone manner in the INS to estimate states but suffers from drifting due to the accumulation of errors through the integration of acceleration and angular velocity measurements [kok2017using, chen2018ionet, brossard2020ai, narasimhappa2019mems]. A commonly adopted localization strategy is coupling GNSS and INS, which combines range measurements from satellites and IMU measurements to calculate states [zhao2016analysis, li2006low, miller2012sensitivity, hu2015derivative]. This coupling strategy alleviates some of the vulnerabilities that standalone GNSS technology faces in urban environments and forested areas [merry2019smartphone].

Though Kalman filters have been the preferred choice for

GNSS and GNSS/INS state estimation for a long time, factor graphs [dellaert2017factor]

have emerged as an alternative framework for solving the localization problem mostly because of the advent of open-source graph optimization libraries like GTSAM 

[dellaert2012factor], g2o [grisetti2011g2o]

. Factor graph optimization has some advantages over the Kalman filters. For example, it uses multiple iterations to solve all states in a batch form instead of just one iteration in a Kalman filter, which is not affected by future measurements (unless smoothing is done). Factor graphs have also been seen to explore better the time correlation between past and current epochs, which helps in outlier removal 

[wen2021factor]. The factor graph framework also makes it easy to add existing and new robust estimation methods  [watson2018evaluation, watson2020robust, pfeifer2018robust] which helps reduce localization failure due to large noise.

In order to provide a cost-effective system, one can utilize the pseudo-measurements in certain conditions from the sensors already on-board. For instance, ZUPT is commonly used as an aiding process for pedestrian navigation [kwakkel2008gnss, zhang2017adaptive]. ZUPT can bound the velocity error and help to calibrate IMU sensor noises [skog2010zero]. This process helps to reduce the INS positioning error growth from cubic to linear since the error state model justifies the correlation between the position and velocity errors of the error covariance matrix. Using ZUPT in state estimation does not need any dedicated sensor to provide zero velocity information, and this information can be obtained by the sensors already on-board (e.g., IMU, wheel encoders). ZUPT requires stationary conditions, and it can be used as an opportunistic navigational update if a wheeled robot stops for other reasons (e.g., obstacle avoidance, re-planning, waiting for pedestrians, stopping at traffic lights). Also, ZUPT can be used to improve wheel odometry (WO)/INS proprioceptive localization with periodic stops in GNSS-denied (or degraded), poor lighting/feature areas [kilic2019improved] and with autonomous stops by deciding when to stop [kilic2020]. The small wheeled robots have more freedom of stopping rather than autonomous cars, which makes utilizing ZUPT a well-suited application for them.

This paper offers the following contributions: 1) detailing a GNSS/WO/INS fusion strategy by exploiting the zero velocity information for both GNSS and INS to be used for wheeled robots, 2) validation of the provided method on actual hardware in field tests with detailed specifications of the implementation and hardware, so the interested reader can easily replicate our work, and 3) making our algorithm publicly available***https://github.com/wvu-navLab/gnss-ins-zupt using open-access data [vz7z-jc84-20].

The rest of this paper is organized as follows. Section II details and explains the components of the algorithm. Section III provides the experimental results. Finally, Section IV provides a conclusion and insights for future works to improve the approach.

Ii Methodology

ii.1 GNSS Factor Graph

The state estimation framework used for processing GNSS data is a factor graph [dellaert2017factor]. Factor graphs solve the maximum a posteriori (MAP) estimation problem by maximizing the product of factors that are probabilistic constraints between states at different time steps and between states and measurements from the current time steps. A depiction of an example factor graph is given in Fig 1.

Figure 1: GNSS factor graph example.

A detailed description of creating factors with GNSS observations is presented in [watson2018evaluation]. The states estimated in the GNSS factor graph are the position, tropospheric delay, clock bias, and phase bias. In Fig. 1, represents any constraint that might exist between the states and the measurements. For example, represents any prior belief on each state, is the motion constraint between two consecutive states along the trajectory, and is the measurement constraint between a state, where the measurements that were perceived from that state. Under the Gaussian assumption, the maximum product of the factors problem converts to a non-linear least squares problem. Following GTSAM nomenclature, we refer to as the between-factor. The cost form of the factors is shown in the equation 2. Each component of the sum is a Mahalanobis cost, the left one being the prior factor cost, the middle one being the between-factor cost( is the measured displacement), and the right one is the GNSS factor cost.


As new GNSS measurements are received, new factors are added using pseudo-range and phase measurements to the existing factor graph and solved incrementally for every epoch using the Incremental Smoothing and Mapping (iSAM2) formulation [kaess2012isam2]. It converts the factor graph to the Bayes tree’s data structure, whose vertices represent cliques for efficient inference. Thus, adding a new constraint to the factor graph only results in the re-linearization of the states within the specific clique.

ii.2 INS/WO Sensor Fusion (CoreNav)

The method in [kilic2019improved] is adopted to improve the proprioceptive localization framework, an error-state extended Kalman filter (EKF) that uses IMU for state propagation and WO for state correction. It also uses ZUPT and non-holonomic constraints as pseudo-measurements to reduce the solution drift. In this INS/WO sensor fusion framework, it is assumed that a small four-wheeled robot uses a set of sensors, including a GNSS receiver and antenna, an IMU, and wheel encoders. The architecture of the INS/WO sensor fusion is provided in Fig. 2. This sensor fusion method is referred to as CoreNav in this paper.

Figure 2: The architecture of the INS/WO sensor fusion (CoreNav) framework. The state error is propagated with IMU and corrected with WO measurements in an error-state EKF framework. ZUPT and non-holonomic update are used to provide valuable information to calibrate the IMU biases. This is done when a ZUPT is triggered with stationary conditions whereas the non-holonomic update is performed for each state.

Following the formulation based on [grovebook, kilic2019improved]

, the error state vector can be constructed in a local navigation frame,


where, is the attitude error, is the velocity error, is the position error, is the IMU acceleration bias, and is the IMU gyroscope bias. For the sake of completeness, the measurement innovations for ZUPT, non-holonomic update, and WO are provided in this section. The detailed derivations and implementation details can be found in [grovebook, kilic2019improved].

ii.2.1 Zero Velocity Update

ZUPT can be used in three ways for wheeled robots: 1) passive ZUPT, as an opportunistic navigational update when rover needs to stop for various reasons, 2) active ZUPT, with periodic stopping [kilic2019improved], and 3) active ZUPT, by deciding when to stop autonomously [kilic2020]. Assuming a stationary rover has zero velocity and zero angular rate (i.e., constant heading), the measurement innovation for a ZUPT can be given as


where is measurement innovation, is estimated velocity vector, and estimated gyro bias.

ii.2.2 Non-Holonomic Update

If a wheeled-robot cannot move sideways with its wheels, then the rover’s velocity is zero along the rotation axis of its wheels, assuming it does not experience side-slip. Additionally, wheeled-robot cannot move perpendicular to the traversal surface. For this reason, these motion constraints can be used as non-holonomic pseudo-measurement update in the filter. Non-holonomic measurement innovation can be given as


where is the coordinate transformation matrix from the body frame to the locally level frame, is rear wheel to body lever arm, and is angular rate measurement.

ii.2.3 Wheel Odometry

In the CoreNav method, the WO measurements are used as an aiding state correction in the error-state EKF. Following the same procedure as described in [kilic2019improved], the measurement innovation for the wheel odometry estimation can be given as


where , , and are estimated longitudinal, lateral and vertical wheel speed, respectively. The subscript denotes the INS estimation, and denotes the WO measurements.

ii.3 GNSS/WO/INS Integration with ZUPT

This section presents two factor graph strategies: 1) utilizing only the zero velocity information in a factor graph method, 2) leveraging the CoreNav position estimates (which are improved with ZUPT and non-holonomic constraints) in a factor graph method. We used the GTSAM library for graph optimization and the standard squared loss function (L2).

The first method does not use INS state estimates in the factor graph directly. It only uses zero velocity signals when INS detects the rover has stopped. To do this, a high certainty zero displacement between-factor , referred to in the Fig. 3 as , is added between the two adjacent state vertices, which are known to be stationary. A low certainty zero-displacement between-factor is also added between non-ZUPT vertices, where the rover is moving, to represent process noise and let the ZUPT information propagate throughout the graph and improve the overall solution. The factors are also referred to as the ZUPT factors in the results section. An illustration of this first method is given in Fig. 3 and called L2-ZUPT.

Figure 3: GNSS factor graph with ZUPT factors, L2-ZUPT

The second method has a more direct coupling between the GNSS and the INS/WO part. Here instead of utilizing the zero velocity information directly in the factor graph, the positioning solution from the CoreNav error state EKF sensor fusion method following [kilic2019improved](see Fig. 2) is used. Note that, in the CoreNav method, the zero velocity information is used as a ZUPT and also includes the non-holonomic constraints to improve the localization solution further. To couple the EKF estimates with the GNSS factor graph, the obtained positioning estimates from the CoreNav method are added as between-factors among all state vertices. The factors are referred to as the CoreNav factors. A depiction of the second method is given in Fig. 4 and called L2-CN.

Figure 4: GNSS factor graph with CoreNav factors, L2-CN.

Iii Experimental Results

iii.1 Robot System Design

In order to collect the data for validating the method, the Pathfinder testbed rover [kilic2019improved] is used. The robot is a skid-steered four-wheeled platform that can be utilized as a testing platform. A depiction of the rover is shown in Fig. 5. The localization sensor suite setup includes a Novatel pinwheel L1/L2 GNSS antenna [novatel2], and receiver[novatel1], an Analog Devices ADIS-16495 IMU [adis], and quadrature wheel encoders. The robot is also equipped with an Intel RealSense T265 camera [intelt265]; however, this sensor is not used in the localization solution. The computer is an Intel NUC Board NUC7i7DN [intelNUC] which hosts an i7-8650U processor. The software runs on the robot under Robot Operating System (ROS) [quigley2009ros].

Figure 5: A small wheeled rover named Pathfinder, in an off-road environment located at Point Marion, PA, where the data were obtained.

iii.2 Field Test Datasets

To evaluate the methods, we use three datasets from [vz7z-jc84-20] referred in the paper as Test 1 (ashpile_mars_analog1.zip), Test 2 (ashpile_mars_analog2.zip) and Test 3 (ashpile_mars_analog3.zip). We also generate a noisy version of these datasets by adding simulated multipath noise based on the elevation of the satellites following the early-minus-late discriminator formulation in [liu2009tracking]. The noise is randomly added to 2 % of the data in each dataset to create the noisy versions. Histogram plots of the simulated range and phase noises are shown in Fig. 6. The rover stops nine times for Test 1, 19 times for Test 2, and 20 times for Test 3 to obtain the zero velocity information. The distances covered in Test 1, 2, and 3 are 671m, 652m, and 663m, respectively. The reference position solutions are obtained by integer-ambiguity-fixed carrier-phase differential GPS (DGPS) processed with RTKLIB [rtklib].

Figure 6: Histogram of simulated multipath phase and range errors[liu2009tracking]

iii.3 Evaluation and Discussion

The three methods compared here are the standard GNSS factor graph (Fig. 1), the GNSS factor graph with ZUPT factors (Fig. 3) and the GNSS factor graph with CoreNav factors (Fig. 4). These are referred to in the figures and tables as L2, L2-ZUPT, and L2-CN, respectively. A comparison of the methods for the clean version of the datasets is given in Table 1. The comparison for the noisy datasets is shown in Table 2. Figures 7 and 8 show the time variation of the errors in the East-North-Up frame. The larger peaks in Fig. 8 in the standard factor graph results are due to the large simulated multipath noise (see Fig. 6.)

Clean Dataset RMSE (m) Max Norm Error (m)
E N U 3D 3D
L2 0.62 0.87 3.82 3.97 5.65
Test 1 L2-ZUPT 0.62 0.34 2.09 2.20 3.06
L2-CN 0.62 0.78 3.34 3.49 4.97
L2 0.49 0.31 1.30 1.43 4.31
Test 2 L2-ZUPT 0.46 0.93 1.24 1.62 2.62
L2-CN 0.51 0.24 0.90 1.06 3.08
L2 0.16 0.92 3.86 3.97 6.14
Test 3 L2-ZUPT 0.16 0.96 3.55 3.69 4.47
L2-CN 0.16 0.92 3.58 3.70 5.65
  • The best results marked up with green boxes

Table 1: Comparison of the methods for the clean datasets.
Figure 7: Time variation of the errors (m) in the East-North-Up frame for clean datasets.
Noisy Dataset RMSE (m) Max Norm Error (m)
E N U 3D 3D
L2 0.88 0.87 2.85 3.10 67.81
Test 1 L2-ZUPT 0.88 0.78 2.27 2.55 22.39
L2-CN 0.67 0.77 2.72 2.90 7.29
L2 0.53 0.40 1.77 1.90 16.12
Test 2 L2-ZUPT 0.59 0.33 1.36 1.52 4.85
L2-CN 0.53 0.24 1.01 1.16 3.41
L2 0.23 0.90 3.59 3.71 7.52
Test 3 L2-ZUPT 0.17 0.93 3.65 3.77 5.08
L2-CN 0.23 0.88 3.55 3.66 6.33
Table 2: Comparison of the methods for noisy datasets.
Figure 8: Time variation of the errors (m) in the East-North-Up frame for noisy datasets.

L2-ZUPT and L2-CN have better performance than the GNSS-only factor graph, L2, for clean and noisy datasets 1 and 2. We see comparable performances from all three methods for dataset 3. L2-ZUPT is found to perform best for clean data, and L2-CN performs best for noisy data. The effect of leveraging zero velocity information can be seen in the noisy data results, where the larger errors in the standard factor graph are dampened by constraining the states during the ZUPTs to be the same since the rover is stationary at that time. The better performance of L2-CN for noisy data can be explained by the fact that the factor graph utilizes more information since it uses the IMU-WO solution from CoreNav. CoreNav also uses additional non-holonomic constraints which are not used in the L2 and L2-ZUPT methods. The performance gap between L2-ZUPT and L2-CN is affected by the number of stops in the datasets. For example, we see a more significant performance gap between L2-ZUPT and L2-CN in Test 1, which has fewer stops. This performance gap is smaller in Test 2, and 3 where the numbers of stops are more than in Test 1, which indicates that using only ZUPT factors in the GNSS factor graph can provide similar localization performance as using a GNSS/WO/INS coupled solution.

Iv Conclusions and Future Work

This work presents GNSS/WO/INS fusion strategies with a factor graph by exploiting the zero velocity information to improve the positioning solution for wheeled robots. The presented methods are compared with a standard GNSS factor graph. This comparison shows that the zero velocity information is a valuable constraint to further improving the positioning solution in a GNSS-degraded environment. Moreover, we observed that using only zero velocity information in a GNSS factor graph can provide comparable positioning performance as using a GNSS/INS/WO coupled position solution. To evaluate our ZUPT aided factor graph methods, we used open access datasets [vz7z-jc84-20]. Also, we made our software implementation publicly available.

Future works will involve testing with more extended datasets with real multipath noise and investigating the connection with robust filtering algorithms. We also experimented with the incremental covariance estimation method discussed in [watson2020robust]. The ZUPT factors are expected to help learn the measurement noise covariance model better. However, the parameter tuning of incremental covariance estimation and ZUPT became a challenge, and due to this, we could not find consistent results with these datasets, which could be attributed to the shorter length of the trajectories.


This work was supported in part by NASA EPSCoR Research Cooperative Agreement WV-80NSSC17M0053, and in part by the Benjamin M. Statler Fellowship.