Author: Jonas Beuchert
This repository accompanies a publication in the proceedings of the IEEE International Conference on Robotics and Automation (ICRA) 2023 where we present an approach to fuse raw GNSS data with other sensing modalities (IMU and lidar) using a factor graph. The goal is to localize a mobile robot in the global Earth frame without drift and discontinuity. The GNSS data is not only used to anchor the robot's trajectory in the global Earth frame and to eliminate drift, but also for highly accurate local positioning using the carrier-phase observations of a GNSS receiver. However, we do not require a base station as differential GNSS methods normally do.
Jonas Beuchert, Marco Camurri, and Maurice Fallon. 2022. Factor Graph Fusion of Raw GNSS Sensing with IMU and Lidar for Precise Robot Localization without a Base Station. ArXiv, 7 pages. https://doi.org/10.48550/arXiv.2209.14649
This repository contains three pieces of work that do not depend on each other:
This repository contains a demo script that shows how carrier-phase observations from a GNSS receiver can be used in factor-graph optimization for accurate local/relative localization of a moving platform. Specifically, we create time-relative double-differential carrier-phase factors between different states in time.
This is not our real-time code. Just a script to demonstrate the concept.
The script is written in Python using the GTSAM library for the optimization and the GPSTk library for the GNSS processing. It was tested with Python 3.7 on Ubuntu 20.04.5 LTS (Focal Fossa). The data comes in u-blox' UBX format.
Open terminal.
apt update
apt install -y cmake git swig wget libtbb-dev libboost-all-dev pip
mkdir raw-gnss-fusion-libs
cd raw-gnss-fusion-libs
wget https://repo.anaconda.com/miniconda/Miniconda3-py37_4.10.3-Linux-x86_64.sh
bash Miniconda3-py37_4.10.3-Linux-x86_64.sh
exit
When prompted, respond with: enter - yes - enter - yes
Might work with other Python versions; just not tested.
(Tested with gpstk v8.0.0
.)
Re-open terminal.
cd raw-gnss-fusion-libs
git clone https://github.com/SGL-UT/GPSTk.git
cd GPSTk
./build.sh -ue
cd ..
Make sure that the installation uses the correct Python version, i.e., the one which you installed in step 2.
(Tested with gtsam v4.1.0
.)
git clone https://github.com/borglab/gtsam.git
cd gtsam
python3.7 -m pip install -r python/requirements.txt
python3.8 -m pip install -r python/requirements.txt
mkdir build
cd build
cmake .. -DGTSAM_BUILD_PYTHON=1 -DGTSAM_PYTHON_VERSION=3.7
make python-install
cd ../..
We need to build from source since rather recent features are required. In the future, just python3.7 -m pip install gtsam
might work.
Make sure that the installation uses the correct Python version, i.e., the one which you installed in step 2. This can be a bit confusing since Ubuntu 20.04's system Python 3.8 will be used in the installation process, but Python 3.7 from step 2 is the target.
git clone https://github.com/JonasBchrt/raw-gnss-fusion.git
python3.7 -m pip install pyubx2==1.1.7 matplotlib numpy geographiclib pymap3d folium
# or
python3.7 -m pip install -r requirements.txt
(Tested with geographiclib v1.52
, pymap3d v2.50
, and folium v0.12.1
.)
cd raw-gnss-fusion
python3.7 demo_doublediff_timerelative_carrierphase_factor_zed_f9p.py
The script:
There are several optimization parameters at the start of the Python script that can be altered.
This sequence is a different one than the one used above. A quadruped robot moved through a dense commercial forest. This is known to be challenging for satellite navigation due to the very limited sky visibility, many outliers in the GNSS measurements because of signal reflections by surrounding vegetation (multi-path effect), and signal degradation caused by the electromagnetic interference of the robot with the GNSS signals.
The data comes as three consecutive rosbags. There is also a RINEX file with the broadcasted satellite navgation data from the trial day.
B_r_BI: [0.516857, 0.043450, -0.092900]
q_BI: [0.999998, 0.000000, -0.001745, 0.000000]
B_r_BL: [0.479112, 0.052000, -0.019785]
q_BL: [0.001234, 0.001234, 0.707106, 0.707106]
B_r_BA: [0.0, 0.0, 0.0]
q_BA: [0.0, 0.0, 0.0, 1.0]
/alphasense_driver_ros/imu
of type sensor_msgs/Imu
/hesai/pandar
of type sensor_msgs/PointCloud2
/ublox_driver/range_meas
of type gnss_comm/GnssMeasMsg
/ublox_driver/receiver_lla
of type sensor_msgs/NavSatFix
or /ublox_driver/receiver_pvt
of type gnss_comm/GnssPVTSolnMsg
0.0014929303436999736
6.883010680707546e-05
0.00029824119649206544
1.2013119882828282e-06
[0, 0, -9.808083883386614]
We implemented pseudorange, IMU, and lidar factors in addition to the carrier-phase factors described above and estimated trajectories on several sequences using the resulting optimization algorithm. This includes the sequence above and further ones that we recorded or that are publicy available.
Trajectory of a car in Hong Kong estimated with our algorithm fusing inertial and raw GNSS measurements (red) in comparison to RTK (ground truth, blue): online map, file. Raw data from GVINS Dataset.
Trajectory of a quadruped robot in the Bagley Wood estimated using inertial, raw GNSS, and lidar data (red) in comparison to RTK (ground truth, blue) and single GNSS fixes (orange): online map, file.
Trajectory of a hand-held GNSS receiver in Oxford estimated using carrier phases only: online map, file.
Trajectory of a car in Oxford estimated with our algorithm fusing inertial, raw GNSS, and lidar data (red) in comparison to RTK (ground truth, blue): online map, file.
Trajectory of a car in Oxford estimated with our algorithm fusing inertial and raw GNSS measurements (red) in comparison to RTK (ground truth, blue) and single GNSS fixes (orange): online map, file.
Jonas Beuchert is supported by the EPSRC Centre for Doctoral Training in Autonomous Intelligent Machines and Systems.