├── LICENSE ├── README.md ├── demo_doublediff_timerelative_carrierphase_factor_zed_f9p.py ├── images └── bagley-setup.jpg ├── navigation_data └── BRDM00DLR_S_20212970000_01D_MN.rnx ├── raw_data └── nhm.ubx ├── requirements.txt ├── results-maps ├── bagley.html ├── bagley.png ├── hong-kong.html ├── hong-kong.png ├── jericho.html ├── jericho.png ├── nhm.html ├── nhm.png ├── park-town.html └── park-town.png └── util.py /LICENSE: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # raw-gnss-fusion 2 | 3 | *Author: Jonas Beuchert* 4 | 5 | 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. 6 | The goal is to localize a mobile robot in the global Earth frame without drift and discontinuity. 7 | 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. 8 | However, we do not require a base station as differential GNSS methods normally do. 9 | 10 | > 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 11 | 12 | This repository contains three pieces of work that do not depend on each other: 13 | 14 | 1. [Demo code for our carrier-phase factors](#demo-code-for-our-carrier-phase-factors) 15 | 16 | 2. [Instructions how to use our public robot dataset with GNSS, IMU, an lidar data](#instructions-how-to-use-our-public-robot-dataset-with-gnss-imu-and-lidar-data) 17 | 18 | 3. [Results of our method on various datasets](#results-of-our-method-on-various-datasets) 19 | 20 | # Demo code for our carrier-phase factors 21 | 22 | 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. 23 | Specifically, we create time-relative double-differential carrier-phase factors between different states in time. 24 | 25 | This is not our real-time code. Just a script to demonstrate the concept. 26 | 27 | ## Setup 28 | 29 | The script is written in Python using the GTSAM library for the optimization and the GPSTk library for the GNSS processing. 30 | It was tested with Python 3.7 on Ubuntu 20.04.5 LTS (Focal Fossa). 31 | The data comes in u-blox' UBX format. 32 | 33 | ### 1. Install dependencies via apt 34 | 35 | Open terminal. 36 | 37 | ```shell 38 | apt update 39 | apt install -y cmake git swig wget libtbb-dev libboost-all-dev pip 40 | ``` 41 | 42 | ### 2. Install Python 3.7 via Miniconda 43 | 44 | ```shell 45 | mkdir raw-gnss-fusion-libs 46 | cd raw-gnss-fusion-libs 47 | wget https://repo.anaconda.com/miniconda/Miniconda3-py37_4.10.3-Linux-x86_64.sh 48 | bash Miniconda3-py37_4.10.3-Linux-x86_64.sh 49 | exit 50 | ``` 51 | When prompted, respond with: *enter* - *yes* - *enter* - *yes* 52 | 53 | Might work with other Python versions; just not tested. 54 | 55 | ### 3. Install GPS Toolkit 56 | 57 | (Tested with `gpstk v8.0.0`.) 58 | 59 | Re-open terminal. 60 | 61 | ```shell 62 | cd raw-gnss-fusion-libs 63 | git clone https://github.com/SGL-UT/GPSTk.git 64 | cd GPSTk 65 | ./build.sh -ue 66 | cd .. 67 | ``` 68 | 69 | Make sure that the installation uses the correct Python version, i.e., the one which you installed in step 2. 70 | 71 | ### 4. Install GTSAM 72 | 73 | (Tested with `gtsam v4.1.0`.) 74 | 75 | ```shell 76 | git clone https://github.com/borglab/gtsam.git 77 | cd gtsam 78 | python3.7 -m pip install -r python/requirements.txt 79 | python3.8 -m pip install -r python/requirements.txt 80 | mkdir build 81 | cd build 82 | cmake .. -DGTSAM_BUILD_PYTHON=1 -DGTSAM_PYTHON_VERSION=3.7 83 | make python-install 84 | cd ../.. 85 | ``` 86 | 87 | We need to build from source since rather recent features are required. In the future, just `python3.7 -m pip install gtsam` might work. 88 | 89 | 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. 90 | 91 | ### 5. Clone this repository and install requirements 92 | 93 | ```shell 94 | git clone https://github.com/JonasBchrt/raw-gnss-fusion.git 95 | python3.7 -m pip install pyubx2==1.1.7 matplotlib numpy geographiclib pymap3d folium 96 | # or 97 | python3.7 -m pip install -r requirements.txt 98 | ``` 99 | 100 | (Tested with `geographiclib v1.52`, `pymap3d v2.50`, and `folium v0.12.1`.) 101 | 102 | ### 6. Run the script 103 | 104 | ```shell 105 | cd raw-gnss-fusion 106 | python3.7 demo_doublediff_timerelative_carrierphase_factor_zed_f9p.py 107 | ``` 108 | 109 | The script: 110 | * Reads raw GNSS data from an UBX file. 111 | * Reads satellite navigation data from a RINEX file. 112 | * Creates a factor graph with a state for every time with at least one GNSS observation. 113 | * Creates double-differenced time-relative carrier-phase factors between different states in time using the carrier-phase obseravtions. 114 | * Solve the factor graph either incrementally or in batch optimization. 115 | * Plots the resulting trajectory. 116 | * Plot a visualization of the factor graph with states as crosses and factors as lines. 117 | * Plots the residuals of the factors. 118 | * Saves a map with the trajectory as HTML file. 119 | 120 | There are several optimization parameters at the start of the Python script that can be altered. 121 | 122 | # Instructions how to use our public robot dataset with GNSS, IMU, and lidar data 123 | 124 | This sequence is a different one than the one used above. 125 | A quadruped robot moved through a dense commercial forest. 126 | 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. 127 | 128 | [Link to dataset.](https://drive.google.com/drive/folders/1VqBe_JhoEwWgVNoIUkqhxmJCWm7UPKhm?usp=sharing) 129 | 130 | The data comes as three consecutive rosbags. 131 | There is also a RINEX file with the broadcasted satellite navgation data from the trial day. 132 | 133 | ### Setup 134 | * Platform: BostonDynamics Spot 135 | * Lidar: HESAI XT32 136 | * IMU: Bosch BMI0856 (part of Sevensense Alphasense) 137 | * GNSS receiver: u-blox C099-F9P 138 | * GNSS antenna: u-blox ANN-MB-00 139 | 140 | ![Quadruped with GNSS receiver, GNSS antenna, and lidar](images/bagley-setup.jpg) 141 | 142 | ### Geometry 143 | * IMU in base frame (position as [x y z] vector): `B_r_BI: [0.516857, 0.043450, -0.092900]` 144 | * IMU in base frame (orientation as [x y z w] quaternion): `q_BI: [0.999998, 0.000000, -0.001745, 0.000000]` 145 | * Lidar in base frame (position as [x y z] vector): `B_r_BL: [0.479112, 0.052000, -0.019785]` 146 | * Lidar in base frame (orientation as [x y z w] quaternion): `q_BL: [0.001234, 0.001234, 0.707106, 0.707106]` 147 | * GNSS antenna in base frame (position as [x y z] vector): `B_r_BA: [0.0, 0.0, 0.0]` 148 | * GNSS antenna in base frame (orientation as [x y z w] quaternion): `q_BA: [0.0, 0.0, 0.0, 1.0]` 149 | 150 | ### Recording rates 151 | * Lidar: 10 Hz 152 | * IMU: 200 Hz 153 | * GNSS: 5 Hz 154 | 155 | ### Recording software 156 | * Operating System: Ubuntu 20.04.5 LTS 157 | * IMU driver: alphasense_driver_ros 158 | * GNSS driver: [ublox_driver](https://github.com/ori-drs/ublox_driver) 159 | 160 | ### ROS topics 161 | * IMU: `/alphasense_driver_ros/imu` of type `sensor_msgs/Imu` 162 | * Lidar: `/hesai/pandar` of type `sensor_msgs/PointCloud2` 163 | * Raw GNSS: `/ublox_driver/range_meas` of type `gnss_comm/GnssMeasMsg` 164 | * GNSS on-board fixes (DGNSS/RTK): `/ublox_driver/receiver_lla` of type `sensor_msgs/NavSatFix` or `/ublox_driver/receiver_pvt` of type `gnss_comm/GnssPVTSolnMsg` 165 | 166 | ### IMU parameters 167 | * Accelerometer noise standard deviation: `0.0014929303436999736` 168 | * Accelerometer bias random walk standard deviation: `6.883010680707546e-05` 169 | * Gyroscope noise standard deviation: `0.00029824119649206544` 170 | * Gyroscope bias random walk sigma: `1.2013119882828282e-06` 171 | * Gravity vector: `[0, 0, -9.808083883386614]` 172 | 173 | ### Notes 174 | * IMU and lidar have a timeshift of 9.062 s w.r.t. the GNSS. 175 | * The GNSS driver has a bug, it reported Galileo signals at frequency 1.2276 GHz, which should be at frequency 1.20714 GHz, the center frequency of Galileo's E5b band. 176 | 177 | # Results of our method on various datasets 178 | 179 | 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. 180 | This includes the sequence above and further ones that we recorded or that are publicy available. 181 | 182 | * 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](https://users.ox.ac.uk/~kell5462/hong-kong.html), [file](results-maps/hong-kong.html). Raw data from [GVINS Dataset](https://github.com/HKUST-Aerial-Robotics/GVINS-Dataset). 183 | ![Car in Hong Kong](results-maps/hong-kong.png) 184 | 185 | * 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](https://users.ox.ac.uk/~kell5462/bagley.html), [file](results-maps/bagley.html). 186 | ![Quadruped in the Bagley Woods](results-maps/bagley.png) 187 | 188 | * Trajectory of a hand-held GNSS receiver in Oxford estimated using carrier phases only: [online map](https://users.ox.ac.uk/~kell5462/nhm.html), [file](results-maps/nhm.html). 189 | ![Hand-held GNSS receiver in Oxford](results-maps/nhm.png) 190 | 191 | * 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](https://users.ox.ac.uk/~kell5462/jericho.html), [file](results-maps/jericho.html). 192 | ![Car in Oxford](results-maps/jericho.png) 193 | 194 | 195 | * 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](https://users.ox.ac.uk/~kell5462/park-town.html), [file](results-maps/park-town.html). 196 | ![Car in Oxford](results-maps/park-town.png) 197 | 198 | ## Funding statement 199 | 200 | Jonas Beuchert is supported by the EPSRC Centre for Doctoral Training in Autonomous Intelligent Machines and Systems. 201 | -------------------------------------------------------------------------------- /demo_doublediff_timerelative_carrierphase_factor_zed_f9p.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Example of a factor graph with time-relative carrier-phase factors. 5 | 6 | Created on Sun Oct 17 12:03:26 2021 7 | 8 | @author: Jonas Beuchert 9 | """ 10 | from pyubx2 import UBXReader 11 | import os 12 | import gpstk 13 | # GTSAM from https://github.com/borglab/gtsam for latest features 14 | # Downloads/gtsam 15 | import gtsam 16 | import matplotlib.pyplot as plt 17 | import numpy as np 18 | from functools import partial # For GTSAM's custom factor 19 | from typing import List, Optional # For type checking in error function of factor 20 | import ctypes # To access content of GPSTk's matrices 21 | from geographiclib.geodesic import Geodesic # Coordinate trafos 22 | import util # Custom utilities 23 | import collections # For queue with maximum length that stores recent observations 24 | import math 25 | 26 | 27 | # Incremental smoothing or offline smoothing 28 | INCREMENTAL = False 29 | # Optimizer type 30 | DOG_LEG = False 31 | # Down-weight previous observations by scaling standard deviation linear with time 32 | WEIGHT = True 33 | # Use Huber loss instead of squared one 34 | HUBER = True 35 | # Use dynamic covariance scaling instaed of squared loss 36 | DCS = False 37 | 38 | if HUBER and DCS: 39 | raise Exception("Can only use Huber loss *or* DCS.") 40 | 41 | # Satellite systems to use 42 | GNSS_LIST = gpstk.vector_GNSS([gpstk.SatelliteSystem.GPS, 43 | gpstk.SatelliteSystem.Glonass, 44 | gpstk.SatelliteSystem.Galileo, 45 | gpstk.SatelliteSystem.BeiDou 46 | ]) 47 | 48 | # Plot factors 49 | PLOT_FACTORS = True # False 50 | 51 | # Desired GNSS obseravtion rate 52 | obs_frequency = 1.0 # [Hz] 53 | 54 | # Meta data 55 | dataset = { 56 | "rhb-handheld": { 57 | "directory": "raw_data", 58 | "observation_file": "nhm.ubx", 59 | "coarse_start_time": gpstk.CivilTime(2021, 10, 24, 11, 0, 0), 60 | "coarse_end_time": gpstk.CivilTime(2021, 10, 24, 11, 30, 0), 61 | "coarse_latitude": 51.759127, 62 | "coarse_longitude": -1.256574, 63 | "coarse_height": 100.0, 64 | "start_key": 0, 65 | "end_key": 113, 66 | "observation_frequency": 1.0, 67 | "temperature": 14.0, # [C] 68 | "pressure": 1017.0, # [mB] 69 | "humidity": 67.0 # [%] 70 | } 71 | } 72 | 73 | # Select dataset 74 | sequence = dataset["rhb-handheld"] 75 | 76 | # Extract data 77 | obs_folder = sequence["directory"] 78 | civil_time = sequence["coarse_start_time"] 79 | obs_file = sequence["observation_file"] 80 | obs_path = os.path.join(obs_folder, obs_file) 81 | step = int(round(sequence["observation_frequency"] / obs_frequency)) 82 | obs_frequency = sequence['observation_frequency'] / step 83 | print(f"Use observation frequency {obs_frequency} Hz.") 84 | start_key = sequence["start_key"] 85 | end_key = sequence["end_key"] 86 | lat0, lon0, h0 = util.read_lat_lon_height_key(obs_path, start_key) 87 | 88 | # Navigation data 89 | ephStore, header = util.read_nav(civil_time) 90 | 91 | # Number of observed/used satellite systems 92 | n_gnss = len(GNSS_LIST) 93 | # State dimension 94 | # Double differences do not require estimation of bias for each GNSS 95 | n_state_vars = 3 96 | # The state to estimate is the 3D position relative to the start position in 97 | # earth center earth fixed (ECEF) coordinates 98 | 99 | # Factor graph 100 | 101 | # Maximum age for carrier phases to create differenced carrier-phase factors [s] 102 | # Cannot generate constraints between long time separated nodes because the 103 | # satellite clock bias changes with time 104 | max_age = 95.0 105 | 106 | # Create initial/reference (geodetic) location and transform to ECEF XYZ 107 | pos_init_geo = gpstk.Position(lat0, lon0, h0, gpstk.Position.Geodetic) 108 | pos_init_ecef = pos_init_geo.transformTo(gpstk.Position.Cartesian) 109 | 110 | # Create noise model, TODO: tune 111 | # Initial receiver clock uncertainty [m] 112 | sigma_delta_t_times_c_prior = 0.02 * gpstk.C_MPS 113 | # Initial location uncertainty in each direction [m] 114 | sigma_pos_prior = 20.0*1e-6 115 | # Uncertainty of initial state 116 | prior_noise = gtsam.noiseModel.Diagonal.Sigmas( 117 | sigma_pos_prior * np.ones(3) 118 | ) 119 | # Prior at origin (relative XYZ coordinates) 120 | prior_mean = np.zeros(3) 121 | # [dx, dy, dz] 122 | 123 | # Create an empty nonlinear factor graph 124 | graph = gtsam.NonlinearFactorGraph() 125 | 126 | # Add a prior on the first point, setting it to the origin 127 | # A prior factor consists of a mean and a noise model (covariance matrix) 128 | prior_factor = gtsam.PriorFactorVector(start_key, prior_mean, prior_noise) 129 | graph.add(prior_factor) 130 | 131 | # Motion model, TODO: tune 132 | # Receiver clock drift noise [m/s] 133 | sigma_delta_t_times_c_drift = 0.1 * gpstk.C_MPS 134 | # Receiver velocity noise in each direction [m/s] 135 | sigma_velocity = 5.0 136 | # Uncertainty of state change [m] 137 | motion_noise = gtsam.noiseModel.Diagonal.Sigmas( 138 | sigma_velocity * np.ones(3) / obs_frequency) 139 | # Random motion (without bias) 140 | motion_mean = np.zeros(n_state_vars) 141 | 142 | # Create the data structure to hold the initial estimate to the solution 143 | initial = gtsam.Values() 144 | 145 | if INCREMENTAL: 146 | # Create (incremental) ISAM2 solver 147 | isam_parameters = gtsam.ISAM2Params() 148 | if DOG_LEG: 149 | isam_parameters.setOptimizationParams(gtsam.ISAM2DoglegParams()) 150 | print() 151 | print("ISAM2 parameters:") 152 | print(isam_parameters) 153 | print() 154 | isam = gtsam.ISAM2(isam_parameters) 155 | previous_state = np.zeros(3) 156 | 157 | # Configure GPSTk helper functions 158 | raim_solver = gpstk.PRSolution() 159 | raim_solver.allowedGNSS = GNSS_LIST 160 | 161 | # Reference ellipsoid for latitude and longitude 162 | geod = Geodesic.WGS84 163 | 164 | # Variable key, start with x_0 165 | key = 0 166 | 167 | # For debugging 168 | debug_info = [] 169 | 170 | def _error_cp(measurement: np.ndarray, 171 | this: gtsam.CustomFactor, 172 | values, 173 | jacobians: Optional[List[np.ndarray]]): 174 | """Time-relative carrier phase factor error function 175 | :param measurement: XYZ satellite locations [m] x4, 176 | receiver clock bias selector vector (binary), 177 | carrier phase difference [m], 178 | to be filled with `partial` 179 | :param this: gtsam.CustomFactor handle 180 | :param values: gtsam.Values 181 | :param jacobians: Optional list of Jacobians 182 | :return: the unwhitened error 183 | """ 184 | state = [values.atVector(key) for key in this.keys()] 185 | receiver_pos = [state[idx][0:3] + np.array([pos_init_ecef[i] for i in range(3)]) for idx in range(2)] 186 | sat_pos = [measurement[(3*idx):(3*idx+3)] for idx in range(4)] 187 | geo_range = [sat_pos[idx] - receiver_pos[int(idx/2)] for idx in range(4)] 188 | geo_range_norm = [np.linalg.norm(geo_range[idx]) for idx in range(4)] 189 | cp = [geo_range_norm[idx] for idx in range(4)] 190 | cp_diff_estimate = (cp[3] - cp[2]) - (cp[1] - cp[0]) 191 | cp_diff_measurement = measurement[-1] 192 | error = cp_diff_estimate - cp_diff_measurement 193 | 194 | if jacobians is not None: 195 | 196 | for idx in range(2): 197 | 198 | unit_vector = [geo_range[idx_] / geo_range_norm[idx_] for idx_ in range(idx*0*2, (idx*0+1)*2)] 199 | 200 | jacobians[idx] = np.array([(-1)**(idx+1) * (-unit_vector[1] - (-unit_vector[0]))]) 201 | 202 | return np.array([error]) 203 | 204 | def weight_cp(std, dt): 205 | return std + dt * std 206 | 207 | # Observations 208 | 209 | # Open UBX file 210 | with open(obs_path, "rb") as stream: 211 | 212 | # UBX file contains only UBX messages, no NMEA messages (adjust if necessary) 213 | ubx_reader = UBXReader(stream, ubxonly=False) 214 | 215 | # Storage for most recent observations 216 | data_storage = {} 217 | 218 | # Loop over all messages 219 | for (raw_data, parsed_data) in ubx_reader: 220 | 221 | # Check if message holds raw observations (like carrier phases) 222 | if parsed_data.identity == "RXM-RAWX": 223 | 224 | # Check if messgae index is in selected data range 225 | if key >= start_key and (key-start_key)%step == 0: 226 | 227 | raw_msg = parsed_data 228 | 229 | # Number of observations 230 | n_obs = raw_msg.__dict__["numMeas"] 231 | # GPS week 232 | week = raw_msg.__dict__["week"] 233 | # Time of week 234 | tow = raw_msg.__dict__["rcvTow"] 235 | # Leap seconds 236 | # leap_sec = raw_msg.__dict__["leapS"] 237 | 238 | # Get timestamp of observation 239 | gps_time = gpstk.GPSWeekSecond(week, 240 | tow, 241 | gpstk.TimeSystem.GPS) 242 | time = gps_time.toCommonTime() # Leap seconds? 243 | 244 | # Loop over all observations for this timestamp/message 245 | for i_obs in range(n_obs): 246 | 247 | # Check if carrier phase measurement is valid 248 | cp_valid = raw_msg.__dict__[f"trkStat_{(i_obs+1):02d}"][0] & 0b00000010 249 | # pr_valid = raw_msg.__dict__[f"trkStat_{(i_obs+1):02d}"][0] & 0b00000001 250 | if cp_valid: 251 | 252 | # Code phase [cycles] 253 | cp = raw_msg.__dict__[f"cpMes_{(i_obs+1):02d}"] 254 | 255 | # System 256 | gnss = raw_msg.__dict__[f"gnssId_{(i_obs+1):02d}"] 257 | # 0: GPS 258 | # 1: SBAS 259 | # 2: Galileo 260 | # 3: BeiDou 261 | # 5: QZSS 262 | # 6: GLONASS 263 | 264 | # Map u-blox GNSS index to GPSTk GNSS index 265 | gnss = util.gnss_map[gnss] 266 | 267 | # Satellite index 268 | sat = raw_msg.__dict__[f"svId_{(i_obs+1):02d}"] 269 | 270 | # Map u-blox satellite index to GPSTk satellite object 271 | sat = gpstk.SatID(sat, gnss) 272 | 273 | # Carrier phase standard deviation [0.004 cycles] 274 | cp_std_raw = raw_msg.__dict__[f"cpStdev_{(i_obs+1):02d}"][0] 275 | cp_std_valid = cp_std_raw < int(0x0F) 276 | if not cp_std_valid: 277 | print(f"State {key}: Invalid standard deviation for {sat}.") 278 | 279 | # Check if navigation data is available for satellite 280 | sat_valid = ephStore.isPresent(sat) 281 | # ...and GNSS shall be used 282 | if sat_valid and gnss in GNSS_LIST and cp_std_valid: 283 | 284 | # Signal type (band) 285 | signal_type = raw_msg.__dict__[f"reserved2_{(i_obs+1):02d}"] 286 | # https://www.u-blox.com/sites/default/files/ZED-F9P_InterfaceDescription_%28UBX-18010854%29.pdf 287 | # 1.5.4 Signal identifiers 288 | 289 | # Carrier frequency [Hz] 290 | carrier_frequency = util.frequency_map[(sat.system, signal_type)] 291 | 292 | # GLONASS frequency slot 293 | if gnss == gpstk.SatelliteSystem.Glonass: 294 | slot = raw_msg.__dict__[f"freqId_{(i_obs+1):02d}"] - 7 295 | frequency_step = util.frequency_step_map[signal_type] 296 | frequency_offset = frequency_step * slot 297 | carrier_frequency += frequency_offset 298 | 299 | # # Carrier-to-noise density ratio (signal strength) [dB-Hz] 300 | # signal_strength = raw_msg.__dict__[f"cno_{(i_obs+1):02d}"] 301 | 302 | # Carrier phase standard deviation [cycles] 303 | cp_std = cp_std_raw * 0.004 304 | 305 | # Carrier phase locktime counter [s] (maximum 64.5 s) 306 | locktime = raw_msg.__dict__[f"locktime_{(i_obs+1):02d}"] * 1e-3 307 | 308 | # Convert carrier phase observations in cycles to carrier phase 309 | # observations in meters 310 | wavelength = gpstk.C_MPS / carrier_frequency 311 | cp = cp * wavelength 312 | 313 | # Get satellite position at transmit time 314 | # and pseudorange corrected for satellite clock 315 | # offset and relativity 316 | svp = gpstk.matrix_double(1, 4) 317 | sat_vec = gpstk.seqToVector([sat], outtype='vector_SatID') 318 | cp_vec = gpstk.seqToVector([cp]) 319 | ok = raim_solver.PreparePRSolution( 320 | Tr=time, 321 | Sats=sat_vec, 322 | Pseudorange=cp_vec, 323 | pEph=ephStore, 324 | SVP=svp) 325 | # CommonTime Tr, vector_SatID Sats, vector_double Pseudorange, XvtStore_SatID pEph, matrix_double SVP) -> int 326 | # SVP = {SV position at transmit time}, raw range + clk + rel 327 | 328 | cp = (ctypes.c_double).from_address(int(svp(0, 3))).value 329 | 330 | # Convert GPSTk matrix to ECEF coordinates 331 | sat_x = (ctypes.c_double).from_address(int(svp(0, 0))).value 332 | sat_y = (ctypes.c_double).from_address(int(svp(0, 1))).value 333 | sat_z = (ctypes.c_double).from_address(int(svp(0, 2))).value 334 | 335 | receiver_x = pos_init_ecef[0] 336 | receiver_y = pos_init_ecef[1] 337 | receiver_z = pos_init_ecef[2] 338 | 339 | # rho [s] 340 | time_of_flight = np.sqrt((sat_x - receiver_x)**2 341 | + (sat_y - receiver_y)**2 342 | + (sat_z - receiver_z)**2) \ 343 | / gpstk.WGS84Ellipsoid().c() 344 | 345 | # Correct for Earth rotation 346 | wt = gpstk.WGS84Ellipsoid().angVelocity() * time_of_flight # [rad] 347 | sat_x = np.cos(wt) * sat_x + np.sin(wt) * sat_y 348 | sat_y = -np.sin(wt) * sat_x + np.cos(wt) * sat_y 349 | 350 | # Add observation to storage 351 | # For use as reference later 352 | # Each observation is identified by satellite ID, 353 | # signal type, and timestamp 354 | 355 | # Check if GNSS and signal type are not already present 356 | if not (gnss, signal_type) in data_storage: 357 | # Create empty dictionary for this combination 358 | data_storage[(gnss, signal_type)] = {} 359 | 360 | # Check if satellite is already present 361 | if not sat in data_storage[(gnss, signal_type)]: 362 | # Create empty queue for this satellite 363 | # (list with finite length) 364 | data_storage[(gnss, signal_type)][sat] = collections.deque( 365 | maxlen=math.ceil(obs_frequency * max_age / step)) 366 | 367 | # Append observation 368 | data_storage[(gnss, signal_type)][sat].append({"key": key, 369 | "time": time, 370 | "locktime": locktime, 371 | "cp": cp, 372 | "cp_std": cp_std, 373 | "sat_x": sat_x, 374 | "sat_y": sat_y, 375 | "sat_z": sat_z}) 376 | 377 | # Remove obsolete observations 378 | while time - data_storage[(gnss, signal_type)][sat][0]["time"] > locktime: 379 | # print(f"Removed obs because locktime is {locktime} s.") 380 | del data_storage[(gnss, signal_type)][sat][0] 381 | 382 | # For each GNSS and band, find satellites that have been visible for the longest time 383 | # Among these satellites, find the one with highest elevation or lowest standard deviation 384 | for gnss, signal_type in data_storage.keys(): 385 | # # (Only use those where at least 2 signals and no cycle slip) 386 | # print(f"GNSS {gnss}, band {signal_type}") 387 | # Iterate over all satellites and delete all data that 388 | # belongs to satellites that are not visible anymore 389 | for sat in list(data_storage[(gnss, signal_type)].keys()): 390 | # Check most recent timestamp 391 | if data_storage[(gnss, signal_type)][sat][-1]["time"] < time: 392 | # Timestamp is old, delete data 393 | del data_storage[(gnss, signal_type)][sat] 394 | # Select sata for all satellites of this GNSS in this signal band 395 | data = data_storage[(gnss, signal_type)] 396 | n_sat = len(data) 397 | if n_sat > 1: 398 | max_locktime = -float("inf") 399 | min_std = float("inf") 400 | best_sat = None 401 | for sat in data.keys(): 402 | sat_data = data[sat] 403 | locktime = len(sat_data) 404 | std = np.mean([sat_data_["cp_std"] for sat_data_ in sat_data]) 405 | if (locktime > max_locktime or (locktime == max_locktime and std < min_std) 406 | ): 407 | best_sat = sat 408 | max_locktime = locktime 409 | min_std = std 410 | if sat_data[-1]["time"] != time: 411 | raise AssertionError("Timestamps does not match.") 412 | 413 | # Iterate over all satellites except for the best one 414 | for sat in data.keys(): 415 | 416 | if sat == best_sat: 417 | # Skip satellite 418 | continue 419 | # Use satellite 420 | # Differencing across satellites 421 | cp_diff_curr = data[sat][-1]["cp"] - data[best_sat][-1]["cp"] 422 | cp_diff_std_curr = np.sqrt(data[sat][-1]["cp_std"]**2 + data[best_sat][-1]["cp_std"]**2) 423 | # Differencing across time 424 | for idx in range(2, len(data[sat]) + 1): 425 | if data[sat][-idx]["time"] != data[best_sat][-idx]["time"]: 426 | print("Timestamps of previous satellite observations do not match.") 427 | continue 428 | cp_diff_old = data[sat][-idx]["cp"] - data[best_sat][-idx]["cp"] 429 | cp_diff = cp_diff_curr - cp_diff_old 430 | # Merge standard devations of current observation 431 | # and reference observation 432 | cp_diff_std_old = np.sqrt(data[sat][-idx]["cp_std"]**2 + data[best_sat][-idx]["cp_std"]**2) 433 | cp_diff_std = np.sqrt(cp_diff_std_curr**2 + cp_diff_std_old**2) 434 | 435 | if WEIGHT: 436 | # Down-weight older sats based on time diff 437 | dt = data[best_sat][-1]["time"] - data[sat][-idx]["time"] 438 | cp_diff_std = weight_cp(cp_diff_std, dt) 439 | 440 | # Create noise model 441 | cp_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([cp_diff_std])) 442 | if HUBER: 443 | cp_noise = gtsam.noiseModel.Robust.Create(gtsam.noiseModel.mEstimator.Huber.Create(1.345), cp_noise) 444 | elif DCS: 445 | cp_noise = gtsam.noiseModel.Robust.Create(gtsam.noiseModel.mEstimator.DCS.Create(1.345), cp_noise) 446 | 447 | # Add time-relative carrierphase factor 448 | # https://github.com/borglab/gtsam/blob/develop/python/gtsam/examples/CustomFactorExample.py 449 | measurement_sat_pos = np.array([data[best_sat][-idx]["sat_x"], 450 | data[best_sat][-idx]["sat_y"], 451 | data[best_sat][-idx]["sat_z"], 452 | data[sat][-idx]["sat_x"], 453 | data[sat][-idx]["sat_y"], 454 | data[sat][-idx]["sat_z"], 455 | data[best_sat][-1]["sat_x"], 456 | data[best_sat][-1]["sat_y"], 457 | data[best_sat][-1]["sat_z"], 458 | data[sat][-1]["sat_x"], 459 | data[sat][-1]["sat_y"], 460 | data[sat][-1]["sat_z"]]) 461 | measurement = np.concatenate((measurement_sat_pos, 462 | np.array([cp_diff]))) 463 | cp_factor = gtsam.CustomFactor( 464 | noiseModel=cp_noise, 465 | keys=[data[sat][-idx]["key"], data[sat][-1]["key"]], 466 | errorFunction=partial( 467 | _error_cp, measurement 468 | ) 469 | ) 470 | graph.add(cp_factor) 471 | debug_info.append({"start_key": data[sat][-idx]["key"], 472 | "end_key": data[sat][-1]["key"], 473 | "start_time": data[sat][-idx]["time"].getSecondOfDay(), 474 | "end_time": data[best_sat][-1]["time"].getSecondOfDay(), 475 | "gnss": gnss, 476 | "signal": signal_type, 477 | "sat": sat}) 478 | 479 | # Add between factor (motion model), except for x_0 480 | if key-step >= start_key: 481 | motion_factor = gtsam.gtsam.BetweenFactorVector( 482 | key - step, key, motion_mean, motion_noise) 483 | graph.add(motion_factor) 484 | 485 | if not INCREMENTAL: 486 | # Add initial value (origin) 487 | initial.insert(key, np.zeros(3)) 488 | 489 | else: 490 | # Add initial value (solution obatined in last valid 491 | # optimization step for last state) 492 | initial.insert(key, previous_state) 493 | # Solve incrementally 494 | try: 495 | try: 496 | isam.update(graph, initial) 497 | except MemoryError: 498 | print(f"Out of memory. Aborting before key {key}.") 499 | break 500 | result = isam.calculateEstimate() 501 | previous_state = result.atVector(key) 502 | 503 | # Find carrier-phase factors 504 | cp_factor_bool_list = np.array([type(graph.at(idx)) is gtsam.CustomFactor 505 | for idx in range(graph.nrFactors())]) 506 | # Find indices of carrier-phase factors 507 | cp_factor_idx_list = np.where(cp_factor_bool_list)[0] 508 | if len(cp_factor_idx_list) > 0: 509 | # Get corresponding residuals 510 | residual_list = np.array([graph.at(idx).unwhitenedError(result)[0] 511 | for idx in cp_factor_idx_list]) 512 | max_residual = np.max(np.abs(residual_list)) 513 | print(f"State {key:>4}: maximum residual {max_residual:.2f} m") 514 | threshold = 1e9 # Threshold for bad observation 515 | if max_residual > threshold: 516 | 517 | # Plot 2D solution 518 | x = [] 519 | y = [] 520 | h = [] 521 | for i in range(start_key, key+step, step): 522 | pos_ecef = gpstk.Position( 523 | result.atVector(i)[0] + pos_init_ecef.X(), 524 | result.atVector(i)[1] + pos_init_ecef.Y(), 525 | result.atVector(i)[2] + pos_init_ecef.Z()) 526 | 527 | g = geod.Inverse(lat0, lon0, 528 | pos_ecef.getGeodeticLatitude(), 529 | pos_ecef.getLongitude()) 530 | x.append(g["s12"] * np.sin(np.pi * g["azi1"] / 180.0)) 531 | y.append(g["s12"] * np.cos(np.pi * g["azi1"] / 180.0)) 532 | h.append(pos_ecef.getAltitude() - h0) 533 | # Plot reference and track 534 | plt.figure(0) 535 | plt.clf() 536 | plt.grid(True) 537 | plt.plot(x, y, "-xr", label="factor-graph solution") 538 | plt.xlabel("east [m]") 539 | plt.ylabel("north [m]") 540 | for i, xi, yi in zip(range(start_key, key+step, step), x, y): 541 | plt.annotate(i, (xi, yi)) 542 | plt.legend(loc="best") 543 | plt.gca().set_aspect("equal", adjustable="datalim") 544 | plt.show() 545 | 546 | debug_info_key = [info for info in debug_info if info["end_key"] == key] 547 | bad_idx = np.where(np.abs(residual_list) > threshold)[0] 548 | print(f"{len(bad_idx)} residuals above threshold {threshold} m.") 549 | for idx in bad_idx: 550 | info_bad = debug_info_key[idx] 551 | print(info_bad) 552 | print(f"Large residual!") 553 | 554 | except RuntimeError as e: 555 | print() 556 | print(f"ISAM cannot update FG at state {key} because of: {e}") 557 | print() 558 | 559 | # Reset 560 | graph = gtsam.NonlinearFactorGraph() 561 | initial.clear() 562 | 563 | # Go to next pose/state 564 | key += 1 565 | 566 | # Check if end of selected data range is reached 567 | if not key < end_key: 568 | break 569 | 570 | end_key = key 571 | 572 | # print("\nFactor Graph:\n{}".format(graph)) 573 | 574 | if not INCREMENTAL: 575 | # Optimize using Levenberg-Marquardt optimization 576 | params = gtsam.LevenbergMarquardtParams() 577 | optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params) 578 | print(f"Initial error: {optimizer.error()}") 579 | print("Optimizing...") 580 | result = optimizer.optimize() 581 | print(f"Final error: {optimizer.error()}") 582 | # print("\nFinal result:\n{}".format(result)) 583 | 584 | x = [] 585 | y = [] 586 | h = [] 587 | 588 | for i in range(start_key, end_key, step): 589 | 590 | pos_ecef = gpstk.Position(result.atVector(i)[0] + pos_init_ecef.X(), 591 | result.atVector(i)[1] + pos_init_ecef.Y(), 592 | result.atVector(i)[2] + pos_init_ecef.Z()) 593 | 594 | g = geod.Inverse(lat0, lon0, 595 | pos_ecef.getGeodeticLatitude(), pos_ecef.getLongitude()) 596 | x.append(g["s12"] * np.sin(np.pi * g["azi1"] / 180.0)) 597 | y.append(g["s12"] * np.cos(np.pi * g["azi1"] / 180.0)) 598 | h.append(pos_ecef.getAltitude() - h0) 599 | 600 | # Plot reference and track 601 | plt.figure(1) 602 | plt.clf() 603 | plt.plot(0, 0, "+b", label="reference location", ms=12) 604 | plt.grid(True) 605 | plt.plot(x, y, "-xr", label="factor-graph solution") 606 | plt.xlabel("east [m]") 607 | plt.ylabel("north [m]") 608 | plt.legend(loc="best") 609 | plt.show() 610 | 611 | if INCREMENTAL: 612 | graph = isam.getFactorsUnsafe() 613 | 614 | if PLOT_FACTORS: 615 | # Plot factors 616 | fig = plt.figure(10) 617 | plt.clf() 618 | plt.plot(0, 0, "+b", label="reference location", ms=12) 619 | plt.grid(True) 620 | # Find carrier-phase factors factors 621 | cp_factor_bool_list = np.array([type(graph.at(idx)) is gtsam.CustomFactor 622 | for idx in range(graph.nrFactors())]) 623 | # Find indices of carrier-phase factors 624 | cp_factor_idx_list = np.where(cp_factor_bool_list)[0] 625 | # Find start and end nodes for the factors of each GNSS 626 | start_key_list = [graph.at(idx).keys()[0] 627 | for idx in cp_factor_idx_list] 628 | end_key_list = [graph.at(idx).keys()[1] 629 | for idx in cp_factor_idx_list] 630 | # Plot factors 631 | cmap = plt.cm.get_cmap('Spectral') 632 | for start_state, end_state in zip(start_key_list, end_key_list): 633 | plt.plot([x[int((start_state - start_key) / step)], 634 | x[int((end_state - start_key) / step)]], 635 | [y[int((start_state - start_key) / step)], 636 | y[int((end_state - start_key) / step)]], "-x", 637 | color=cmap((start_state - start_key) / (end_key - start_key)), 638 | linewidth=0.5) 639 | plt.xlabel("east [m]") 640 | plt.ylabel("north [m]") 641 | plt.title("binary carrier-phase factors") 642 | plt.gca().set_aspect("equal", adjustable="datalim") 643 | plt.show() 644 | 645 | plt.figure(2) 646 | plt.clf() 647 | for ecef_idx in range(3): 648 | plt.subplot(1, 3, ecef_idx + 1) 649 | plt.grid(True) 650 | plt.xlabel("variable key") 651 | plt.ylabel(f"{['x', 'y', 'z'][ecef_idx]} [m]") 652 | plt.plot([key for key in range(start_key, end_key, step)], 653 | [result.atVector(key)[ecef_idx] for key in range(start_key, end_key, step)], 654 | "-x") 655 | plt.title(['ECEF - x', 'ECEF - y', 'ECEF - z'][ecef_idx]) 656 | plt.show() 657 | 658 | # Plot residuals 659 | plt.figure(3) 660 | plt.clf() 661 | # Find carrier-phase factors factors 662 | cp_factor_bool_list = np.array([type(graph.at(idx)) is gtsam.CustomFactor 663 | for idx in range(graph.nrFactors())]) 664 | # Find indices of carrier-phase factors 665 | cp_factor_idx_list = np.where(cp_factor_bool_list)[0] 666 | if len(debug_info) == 0: 667 | # Find GNSSs of carrier-pase factors 668 | gnss_selector_list = np.array([np.where(graph.at(idx).linearize(result).jacobian()[0][0][3:(3+len(GNSS_LIST))])[0][0] 669 | for idx in cp_factor_idx_list]) 670 | # Find start and end nodes for the factors of each GNSS 671 | start_key_list = [np.array([graph.at(cp_factor_idx_list[idx]).keys()[0] 672 | for idx in np.where(np.array(gnss_selector_list)==gnss_idx)[0]]) 673 | for gnss_idx in range(len(GNSS_LIST))] 674 | end_key_list = [np.array([graph.at(cp_factor_idx_list[idx]).keys()[1] 675 | for idx in np.where(np.array(gnss_selector_list)==gnss_idx)[0]]) 676 | for gnss_idx in range(len(GNSS_LIST))] 677 | # Get corresponding residuals 678 | residual_list = [np.array([graph.at(cp_factor_idx_list[idx]).unwhitenedError(result)[0] 679 | for idx in np.where(np.array(gnss_selector_list)==gnss_idx)[0]]) 680 | for gnss_idx in range(len(GNSS_LIST))] 681 | else: 682 | # Find GNSSs of carrier-pase factors 683 | gnss_selector_list = np.array([info["gnss"] 684 | for info in debug_info if info["end_key"] < end_key]) 685 | # Find start and end nodes for the factors of each GNSS 686 | start_key_list = [np.array([graph.at(cp_factor_idx_list[idx]).keys()[0] 687 | for idx in np.where(gnss_selector_list==gnss)[0]]) 688 | for gnss in GNSS_LIST] 689 | end_key_list = [np.array([graph.at(cp_factor_idx_list[idx]).keys()[1] 690 | for idx in np.where(gnss_selector_list==gnss)[0]]) 691 | for gnss in GNSS_LIST] 692 | # Get corresponding residuals 693 | residual_list = [np.array([graph.at(cp_factor_idx_list[idx]).unwhitenedError(result)[0] 694 | for idx in np.where(gnss_selector_list==gnss)[0]]) 695 | for gnss in GNSS_LIST] 696 | # Find problematic satellites 697 | threshold = 1.0 # Threshold for bad observation 698 | info_list = [[info for info in debug_info if info["gnss"] == gnss] 699 | for gnss in GNSS_LIST] 700 | bad_cp = [[info[bad_idx] for bad_idx in np.where(res > threshold)[0]] for res, info in zip(residual_list, info_list)] 701 | bad_res_cp = [[res[bad_idx] for bad_idx in np.where(res > threshold)[0]] for res, info in zip(residual_list, info_list)] 702 | for i_gnss, r_gnss in zip(bad_cp, bad_res_cp): 703 | print() 704 | for i, r in zip(i_gnss, r_gnss): 705 | print(f"Large cp residual between x_{i['start_key']} ({i['start_time']:.1f} s) and x_{i['end_key']} ({i['end_time']:.1f} s) for {i['sat']}, {util.band_map[(i['gnss'], i['signal'])].name}: {r:.1f} m") 706 | # Plot residuals for each GNSS 707 | for gnss_idx in range(len(GNSS_LIST)): 708 | plt.scatter(start_key_list[gnss_idx], 709 | residual_list[gnss_idx], 710 | marker="+", label=gpstk.SatelliteSystem(GNSS_LIST[gnss_idx]).name) 711 | plt.title("residuals (start key)") 712 | plt.ylabel("unwhitened error [m]") 713 | plt.xlabel("variable key") 714 | plt.grid() 715 | plt.legend() 716 | plt.show() 717 | # 3D plot 718 | # ax = plt.figure(4).add_subplot(projection='3d') 719 | # for gnss_idx in range(len(GNSS_LIST)): 720 | # n = len(end_key_list[gnss_idx]) 721 | # ax.scatter(xs=start_key_list[gnss_idx], 722 | # ys=end_key_list[gnss_idx], 723 | # zs=residual_list[gnss_idx], 724 | # label=gpstk.SatelliteSystem(GNSS_LIST[gnss_idx]).name) 725 | # plt.title("residuals") 726 | # ax.set_zlabel("unwhitened error [m]") 727 | # ax.set_xlabel("start key") 728 | # ax.set_ylabel("end key") 729 | # plt.legend() 730 | # plt.show() 731 | 732 | # Plot residuals (sorted by end key) 733 | plt.figure(5) 734 | plt.clf() 735 | # Plot residuals for each GNSS 736 | for gnss_idx in range(len(GNSS_LIST)): 737 | plt.scatter(end_key_list[gnss_idx], 738 | residual_list[gnss_idx], 739 | marker="+", label=gpstk.SatelliteSystem(GNSS_LIST[gnss_idx]).name) 740 | plt.title("residuals (end key)") 741 | plt.ylabel("unwhitened error [m]") 742 | plt.xlabel("variable key") 743 | plt.grid() 744 | plt.legend() 745 | plt.show() 746 | 747 | # Plot as HTML map 748 | map_name = obs_file[:-4] + "-DD-CP" 749 | util.plot_map(result, lat0, lon0, pos_init_ecef, name=map_name) 750 | print(f"Saved map as {map_name}.") 751 | -------------------------------------------------------------------------------- /images/bagley-setup.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JonasBchrt/raw-gnss-fusion/4b61fed95c08578eb115a15a681451eee8527287/images/bagley-setup.jpg -------------------------------------------------------------------------------- /raw_data/nhm.ubx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JonasBchrt/raw-gnss-fusion/4b61fed95c08578eb115a15a681451eee8527287/raw_data/nhm.ubx -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | pyubx2==1.1.7 2 | matplotlib 3 | numpy 4 | geographiclib 5 | pymap3d 6 | folium 7 | -------------------------------------------------------------------------------- /results-maps/bagley.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JonasBchrt/raw-gnss-fusion/4b61fed95c08578eb115a15a681451eee8527287/results-maps/bagley.png -------------------------------------------------------------------------------- /results-maps/hong-kong.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JonasBchrt/raw-gnss-fusion/4b61fed95c08578eb115a15a681451eee8527287/results-maps/hong-kong.png -------------------------------------------------------------------------------- /results-maps/jericho.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JonasBchrt/raw-gnss-fusion/4b61fed95c08578eb115a15a681451eee8527287/results-maps/jericho.png -------------------------------------------------------------------------------- /results-maps/nhm.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 25 | 34 | 35 | 36 | 37 | 38 |
39 | 40 | 41 | 77 | -------------------------------------------------------------------------------- /results-maps/nhm.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JonasBchrt/raw-gnss-fusion/4b61fed95c08578eb115a15a681451eee8527287/results-maps/nhm.png -------------------------------------------------------------------------------- /results-maps/park-town.html: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 25 | 34 | 35 | 36 | 37 | 38 |
39 | 40 | 41 | -------------------------------------------------------------------------------- /results-maps/park-town.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/JonasBchrt/raw-gnss-fusion/4b61fed95c08578eb115a15a681451eee8527287/results-maps/park-town.png -------------------------------------------------------------------------------- /util.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | """ 4 | Created on Fri Oct 1 10:03:34 2021 5 | 6 | @author: jonasbeuchert 7 | """ 8 | from pyubx2 import UBXReader 9 | from pyubx2.exceptions import UBXParseError 10 | import gpstk 11 | import os 12 | import folium 13 | 14 | 15 | # Map u-blox GNSS indices to GPSTk GNSS index 16 | # https://www.u-blox.com/sites/default/files/ZED-F9P_InterfaceDescription_%28UBX-18010854%29.pdf 17 | # 1.5.2 GNSS identifiers 18 | gnss_map = {0: gpstk.SatelliteSystem.GPS, 19 | 1: gpstk.SatelliteSystem.Geosync, 20 | 2: gpstk.SatelliteSystem.Galileo, 21 | 3: gpstk.SatelliteSystem.BeiDou, 22 | 5: gpstk.SatelliteSystem.QZSS, 23 | 6: gpstk.SatelliteSystem.Glonass} 24 | 25 | # Map u-blox signal identifiers to GPSTk 26 | # https://www.u-blox.com/sites/default/files/ZED-F9P_InterfaceDescription_%28UBX-18010854%29.pdf 27 | # 1.5.4 Signal identifiers 28 | frequency_map = {(gpstk.SatelliteSystem.GPS, 0): gpstk.FREQ_GPS_L1, 29 | (gpstk.SatelliteSystem.GPS, 3): gpstk.FREQ_GPS_L2, 30 | (gpstk.SatelliteSystem.GPS, 4): gpstk.FREQ_GPS_L2, 31 | (gpstk.SatelliteSystem.Geosync, 0): gpstk.FREQ_SBAS_L1, 32 | (gpstk.SatelliteSystem.Galileo, 0): gpstk.FREQ_GALILEO_E1, 33 | (gpstk.SatelliteSystem.Galileo, 1): gpstk.FREQ_GALILEO_E1, 34 | (gpstk.SatelliteSystem.Galileo, 5): gpstk.FREQ_GALILEO_E5b, 35 | (gpstk.SatelliteSystem.Galileo, 6): gpstk.FREQ_GALILEO_E5b, 36 | (gpstk.SatelliteSystem.BeiDou, 0): gpstk.FREQ_BEIDOU_B1, 37 | (gpstk.SatelliteSystem.BeiDou, 1): gpstk.FREQ_BEIDOU_B1, 38 | (gpstk.SatelliteSystem.BeiDou, 2): 1207.14e6, 39 | (gpstk.SatelliteSystem.BeiDou, 3): 1207.14e6, 40 | (gpstk.SatelliteSystem.QZSS, 0): gpstk.FREQ_QZSS_L1, 41 | (gpstk.SatelliteSystem.QZSS, 1): gpstk.FREQ_QZSS_L1, 42 | (gpstk.SatelliteSystem.QZSS, 4): gpstk.FREQ_QZSS_L2, 43 | (gpstk.SatelliteSystem.QZSS, 5): gpstk.FREQ_QZSS_L2, 44 | (gpstk.SatelliteSystem.Glonass, 0): gpstk.FREQ_GLONASS_G1, 45 | (gpstk.SatelliteSystem.Glonass, 2): gpstk.FREQ_GLONASS_G2 46 | } 47 | 48 | # Map u-blox GLONASS frequency step identifiers to GPSTk 49 | frequency_step_map = {0: gpstk.L1_FREQ_STEP_GLO, 50 | 2: gpstk.L2_FREQ_STEP_GLO} 51 | 52 | # Map u-blox signal band identifiers to GPSTk 53 | band_map = {(gpstk.SatelliteSystem.GPS, 0): gpstk.CarrierBand.L1, 54 | (gpstk.SatelliteSystem.GPS, 3): gpstk.CarrierBand.L2, 55 | (gpstk.SatelliteSystem.GPS, 4): gpstk.CarrierBand.L2, 56 | (gpstk.SatelliteSystem.Geosync, 0): gpstk.CarrierBand.L1, 57 | (gpstk.SatelliteSystem.Galileo, 0): gpstk.CarrierBand.L1, 58 | (gpstk.SatelliteSystem.Galileo, 1): gpstk.CarrierBand.L1, 59 | (gpstk.SatelliteSystem.Galileo, 5): gpstk.CarrierBand.E5b, 60 | (gpstk.SatelliteSystem.Galileo, 6): gpstk.CarrierBand.E5b, 61 | (gpstk.SatelliteSystem.BeiDou, 0): gpstk.CarrierBand.B1, 62 | (gpstk.SatelliteSystem.BeiDou, 1): gpstk.CarrierBand.B1, 63 | (gpstk.SatelliteSystem.BeiDou, 2): gpstk.CarrierBand.B2, 64 | (gpstk.SatelliteSystem.BeiDou, 3): gpstk.CarrierBand.B2, 65 | (gpstk.SatelliteSystem.QZSS, 0): gpstk.CarrierBand.L1, 66 | (gpstk.SatelliteSystem.QZSS, 1): gpstk.CarrierBand.L1, 67 | (gpstk.SatelliteSystem.QZSS, 4): gpstk.CarrierBand.L2, 68 | (gpstk.SatelliteSystem.QZSS, 5): gpstk.CarrierBand.L2, 69 | (gpstk.SatelliteSystem.Glonass, 0): gpstk.CarrierBand.G1, 70 | (gpstk.SatelliteSystem.Glonass, 2): gpstk.CarrierBand.G2 71 | } 72 | 73 | 74 | def read_lat_lon_height_key(file_path, key=0): 75 | """ 76 | Read .ubx file and return latitude, longitude of reference solution. 77 | 78 | Parameters 79 | ---------- 80 | file_path : str 81 | Path to .ubx file. 82 | key : int, optional 83 | Index of location to return. The default is 0. 84 | 85 | Returns 86 | ------- 87 | latitude : float 88 | Latitude [decimal degrees] 89 | longitude : float 90 | Latitude [decimal degrees] 91 | height : float 92 | Height above ellipsoid [m] 93 | 94 | Author: Jonas Beuchert 95 | """ 96 | # Loop counter 97 | idx = -1 98 | # Read geodetic coordinates from file 99 | with open(file_path, "rb") as stream: 100 | ubr = UBXReader(stream, ubxonly=False) 101 | while idx < key: 102 | try: 103 | (raw_data, parsed_data) = next(ubr) 104 | if parsed_data.identity == "NAV-PVT": 105 | idx += 1 106 | except StopIteration: 107 | raise ValueError(f"Could not find latitude/lomgitude for key {key} in {file_path}.") 108 | except UBXParseError as msg: 109 | print(msg) 110 | # Convert to degrees 111 | lat = parsed_data.lat * 1e-7 112 | lon = parsed_data.lon * 1e-7 113 | height = parsed_data.height * 1e-3 114 | return lat, lon, height 115 | 116 | 117 | def read_nav(civil_time, nav_folder="", provider="IGS"): 118 | """ 119 | Read navigation data from existing RINEX file or download it. 120 | 121 | Parameters 122 | ---------- 123 | civil_time : gpstk.CivilTime 124 | Coarse time. 125 | nav_folder : str, optional 126 | Path to directory with navigation data files. The default is 127 | 'navigation_data'. 128 | provider : str, optional 129 | 'IGS' or 'MGEX'. The default is 'IGS'. 130 | 131 | Returns 132 | ------- 133 | eph_store : gpstk.Rinex3EphemerisStore 134 | Ephemerides. 135 | nav_header : gpstk.Rinex3NavHeader 136 | Navigation data header without ionospheric corrections and time system 137 | corrections. 138 | 139 | Author: Jonas Beuchert 140 | """ 141 | import requests 142 | import gzip 143 | import re 144 | 145 | 146 | if nav_folder == "": 147 | nav_folder = "navigation_data" 148 | 149 | if provider not in ["IGS", "MGEX"]: 150 | raise ValueError(f"Provider must be 'IGS' or 'MGEX', but is {provider}.") 151 | 152 | # Determine file name 153 | day_of_year = gpstk.YDSTime(civil_time).doy 154 | if provider == "IGS": 155 | file_name_start = "BRDM00DLR" 156 | elif provider == "MGEX": 157 | file_name_start = "BRDC00WRD" 158 | nav_file = f"{file_name_start}_S_{civil_time.year:04d}{day_of_year:03d}0000_01D_MN.rnx" 159 | 160 | # Determine file path 161 | nav_path = os.path.join(nav_folder, nav_file) 162 | 163 | try: 164 | # Read file 165 | nav_header, nav_data = gpstk.readRinex3Nav(nav_path) 166 | except OSError: 167 | # Download link for broadcasted ephemerides 168 | url = f"https://igs.bkg.bund.de/root_ftp/IGS/BRDC/{civil_time.year:04d}/{day_of_year:03d}/{nav_file}.gz" 169 | # Download 170 | response = requests.get(url) 171 | # Uncompress 172 | bytes = gzip.decompress(response.content) 173 | txt = str(bytes.decode()) 174 | # Clean up file to be processable by gpstk.readRinex3Nav() 175 | for keyword_0_list, keyword_1 in zip([[""], ["BDSA", "BDSB", "QZSA", "QZSB", "IRNA", "IRNB"], [""]], ["MERGED_FILE", "IONOSPHERIC CORR", "TIME SYSTEM CORR"]): 176 | for keyword_0 in keyword_0_list: 177 | txt = re.sub(pattern=f"\n{keyword_0}(.*){keyword_1}( *)", repl="", string=txt) 178 | # Write to file 179 | with open(nav_path, 'w') as f_out: 180 | f_out.write(txt) 181 | # Read file 182 | nav_header, nav_data = gpstk.readRinex3Nav(nav_path) 183 | 184 | # Store ephemerides 185 | eph_store = gpstk.gpstk.Rinex3EphemerisStore() 186 | for nav_data_obj in nav_data: 187 | try: 188 | eph_store.addEphemeris(nav_data_obj) 189 | except gpstk.InvalidParameter: 190 | print("Could not add ephemeris because of invalid value.") 191 | 192 | return eph_store, nav_header 193 | 194 | 195 | def plot_map(result, lat_ref, lon_ref, pos_init_ecef, name="temp", 196 | terrain=False): 197 | # Create map 198 | m = folium.Map( 199 | location=[lat_ref, lon_ref], 200 | tiles="OpenStreetMap" if not terrain else "http://tile.stamen.com/terrain-background/{z}/{x}/{y}.png", 201 | attr=' ', 202 | control_scale=True, 203 | zoom_control=False, 204 | zoom_start=18 205 | ) 206 | # Assemble estimated path 207 | pos_ecef_list = [gpstk.Position(result.atVector(i)[0] + pos_init_ecef[0], 208 | result.atVector(i)[1] + pos_init_ecef[1], 209 | result.atVector(i)[2] + pos_init_ecef[2]) 210 | for i in result.keys()] 211 | path = [(pos_ecef.getGeodeticLatitude(), 212 | -360.0 + pos_ecef.getLongitude()) 213 | for pos_ecef in pos_ecef_list] 214 | # Draw path 215 | folium.PolyLine(path, weight=3, color="red").add_to(m) 216 | # Draw reference point 217 | folium.Marker([lat_ref, lon_ref]).add_to(m) 218 | # Save map 219 | m.save(f"{name}.html") 220 | try: 221 | display(m) 222 | except Exception as e: 223 | print("To display the map, run the script with IPython/jupyter.") 224 | --------------------------------------------------------------------------------