I’ve recently been doing some consulting work for Seescan, a company that provides equipment for locating underground utilities. The goal was to help them improve their precision positioning in challenging conditions. In particular, they tend to see rapid transitions from reasonably open sky conditions to very challenging conditions as the operator may follow a utility under a roof or overhang, or even inside a building for a short time. Their equipment already included a GNSS receiver with RTK or PPP-RTK corrections and an IMU, so the goal was to take advantage of the IMU to dead reckon through relatively short intervals where the GNSS signals were very poor or non-existent. As part of that project, I developed for them a tool written in python to help with the development and testing of their internal sensor-fusion code. Seescan has generously allowed me to share that code as an open-source project.
The result is the GNSS_IMU repository on Github. It is based on Matlab code that Paul Groves created as a supplement to his well-known text book, “Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems”. It is a loosely coupled solution, meaning that it uses the output of the GNSS solution as an input rather than the raw GNSS measurements. This makes it more compatible with their existing solution and easier to implement. In this post, I would like to give a quick introduction and demonstration of the code.
The original Matlab code from Paul Groves is a full simulator of both the GNSS solution and the IMU solution and includes the capability to generate simulated GNSS and IMU data from a set of position and orientation data. It includes both loosely and tightly coupled solutions. From this larger library, I have extracted just the loosely coupled solution but have added some features including lever-arm corrections, zero-velocity updates, initial yaw-alignment using GNSS heading, non-holonomic constraint updates, IMU scale factors, and simulated GNSS outages. Since I am primarily interested in post-processing and near-real time solutions rather than true real-time solutions, I have also added options for backwards solutions or combined forward/backward solutions, as well as first-fix backward smoothing after GNSS outages.
The code is based on a mechanization function for the IMU and an error-state Kalman filter for the sensor fusion with 15 default states and 6 optional states . The error-state Kalman filter tracks deviations from a nominal trajectory rather than the full states, because these errors remain small and approximately linear, especially for orientation. The 15 default states include three for orientation, three for position, three for velocity, three for accelerometer biases, and three for gyro biases. The six optional states include three for accelerometer scale factors and three for gyro scale factors.
Following the original MATLAB implementation, the code comments include references to relevant equations from the Groves textbook where possible.
I have also included two sample data sets. The first is from a vehicle driving on hilly residential streets followed by tight maneuvers in a parking lot. The second is from a handheld receiver while walking around in a backyard setting. Both data sets were collected with a u-blox X20P GNSS receiver in RTK mode and a TDK ICM-45686 IMU. The ICM-45686 is an inexpensive, roughly five-dollar consumer-grade IMU that still outperforms many of the lowest cost options in its class.
Like my python version of RTKLIB, this code is designed to get your hands dirty and dig directly into the code, so it does not have a slick GUI interface like RTKLIB. However, it does use RTKLIB format solution files for both the GNSS input solution and the combined GNSS/IMU output solution, so RTKPLOT can be used to explore and compare the input and output solutions and POS2KML can be used to easily generate KML files from input or output solution files to further explore with Google Earth.
The input files and configuration parameter file are specified in the header of the main python script, GNSS_IMU.py. The header is setup so that either sample data set can be run by commenting/uncommenting lines in the header.
########## Data selection and run configuration ########################################
# Uncomment these lines to run an example of walking with a handheld GNSS receiver
# import walk_config as cfg
# dataDir = r'..\data\walk_0827'
# fileIn = '1730_sf'
# Uncomment these lines to run an example of driving with a roof mounted GNSS receiver
import drive_config as cfg
dataDir = r'..\data\drive_0708'
fileIn = '1934_sf'To run your own data, you would modify this code to point to your data files. I have included a couple of python scripts to help convert IMU and GNSS data collected with the RTKLIB stream-servers with time-tags enabled into the correct format but I will leave discussion of those for a future post.
Here’s a list of the configurable parameters for a given solution. This particular example is for the driving sample data.
# IMU parameters
imu_sample_rate = 100
imu_offset = [0., 0., -0.65] # offset from system origin: forward, right, down
gyro_noise_PSD = 0.0038 # deg/sec/sqrt(Hz)
accel_noise_PSD = 70 # ug/sqrt(Hz)
accel_bias_PSD = 7 # ug/sqrt(Hz)
gyro_bias_PSD = 3.8e-5 # deg/sec^2/sqrt(Hz)
accel_scale_noise_SD = 0.7
gyro_scale_noise_SD = 3.8e-6
imu_misalign = np.array([180.0, 0.0, 180.0]) # IMU orientation
imu_misalign += np.array([0, -6.79, 5.35]) # IMU misalign to body frame (degrees rpy)
# GNSS parameters
gnss_offset = [0, -0.05, -0.65] # offset from system origin: forward, right, down (m)
# Magnetometer parameters
mag_enable = False
# ratio of specs to process noise stdevs to account for unmodeled errors
imu_noise_factors = [1, 2, 4, 2, 0.25, 0.25] # attitude, velocity, accel bias, gyro bias, accel scale, gyro scale
gnss_noise_factors = [1, 1] # position, velocity
# Initial uncertainties
init =Init()
init.att_unc = [10, 10, 100] # initial attitude uncertainty per axis in (deg)
init.vel_unc = [0.05, 0.05, 0.1] # initial velocity uncertainty per axis (m/s)
init.pos_unc = [0.05, 0.05, 0.1] # initial position uncertainty per axis (m)
init.bias_acc_unc = 0.2 # initial accel bias uncertainty (m/sec^2)
init.bias_gyro_unc = 0.2 # initial gyro bias uncertainty (deg/sec)
init.scale_acc_unc = 0.001
init.scale_gyro_unc = 0.001
# Run specific parameters
scale_factors = False # Kalman filter scale factor states enabled
imu_t_off = -0.125 # time to add to IMU time stamps to acccount for logging delay (sec)
run_dir = [1] # run direction ([-1]=backwards, [1]=forwards, [-1,1] = combined)
gnss_epoch_step = 1 # GNSS input sample decimation (1=use all GNSS samples)
out_step = 1 # Output decimation [1=include all samples]
float_err_gain = 2 # mulitplier on pos/vel stdevs if in float mode
single_err_gain = 5 # mulitplier on pos/vel stdevs if in single mode
# Velocity matching
vel_match = True # do velocity matching at end of coast
vel_match_min_t = 1 # min GNSS outage to invoke vel match (seconds)
# Zero Velocity update
zupt_enable = True
zupt_epcoh_count = 50
zupt_accel_thresh = 0.25 # m/sec^2
zupt_gyro_thresh = 0.25 # deg/sec
zupt_vel_SD = 0.01 # standard dev (m/sec)
zaru_gyro_SD = 0.01 # standard dev (deg/sec)
# Initial yaw alignment
yaw_align = True # use GNSS heading to initialize yaw
yaw_align_min_vel = 1.0 # min vel (m/sec)
yaw_off = 0 # adjustment from initial yaw or mag (deg), just needs to be approximate
init_yaw_with_mag = False
# Non-holomonic constrains (NHC) update
nhc_enable = False
nhc_epcoh_count = 100
nhc_min_vel = 1.0
nhc_gyro_thesh = 20 # deg/sec
nhc_vel_SD = .25 # standard dev m/sec
nhc_vel_SD_coast = 0.05 # standard dev m/sec
# Testing/Debug
disable_imu = False # enable to Run GNSS only
start_coast = 40 # start of simulated GNSS outages (secs)
end_coast = 30 # end of simulated GNSS outages (secs before end)
coast_len = 15 # length of simulated GNSS outages (secs)
num_epochs = 0 # num epochs to run (0=all)
gyro_bias_err = [0, 0, 0] # Add constant error to gyro biases (deg/sec)
accel_bias_err = [0, 0, 0] # Add constant error to acc biases (m/sec^2)
gyro_scale_factor = [1, 1, 1]
accel_scale_factor = [1, 1, 1]
# Plotting
plot_results = True
plot_bias_data = True # plot accel and gyro bias states
plot_imu_data = False # plot accel, gyro raw data
plot_unc_data = False # plot Kalman filter uncertaintiesIn this post, I am not going to get in-depth into the different parameters, I am just going to demonstrate running the sample driving dataset under a few different configurations.
To start with, I will run GNSS_IMU.py without changing any parameters in the configuration file (drive_config.py). This is setup to run with 15 second long GNSS outages separated by 30 second intervals of valid GNSS data. It is what I call a near-real time solution, not a real-time solution, which means it uses the first fixed GNSS solution point after the outage to adjust the position/velocity trajectories during the outage. This is the “velocity matching” option in the configuration file. For applications, that don’t require a true real-time solution, this significantly reduces the errors during the GNSS outages. This mode provides initial position/velocity estimates in true real-time during the outage and then, immediately after the first post-outage fix, updated position/velocity for all of the outage epochs.
Running the script will generate an output solution file in RTKLIB format and a number of plots. The first two plots (below) indicate the position and velocity for the input solution without GNSS outages, and the loosely coupled sensor fusion output solution with GNSS outages. The blue/red/green traces are north/east/down for the input GNSS solution, the red/purple/brown are the sensor fusion solution with valid GNSS data and the pink/gray/yellow are the sensor fusion solution during the outages.
To estimate the solution errors during the GNSS outages, I use the input solution which doesn’t include the GNSS outages as a ground truth, and estimate the error to be the difference between the input solution without GNSS outages and the output solution with GNSS outages.
The fifth plot in the script output (below) shows these differences: The peaks in these plots indicate the maximum error during the 15 second GNSS outages. Remember that this is after the trajectory corrections done using the first fixed point after each outage, the errors would be significantly larger without these corrections.
The fourth plot in the set of output plots compares the solution orientation with the GNSS heading derived from velocity for all epochs with a velocity over a specified minimum velocity. For applications like a vehicle, which primarily moves in a single direction in the body frame, these should be similar provided the yaw is properly initialized. In this configuration, the “yaw_align” option is enabled which means that the first GNSS heading over a specified velocity is used to initialize the yaw state of the Kalman filter. Note that the velocity heading in this plot is derived from input velocity at the GNSS antenna which means it doesn’t compensate for any lever-arm between the GNSS antenna and the vehicle origin which is usually defined as the center of the rear axle, so these can differ by a small amount. In the case of a handheld receiver the two will tend to differ by much more since the orientation and direction of travel are more independent. R,P, and Y in the plot legend refer to roll, pitch, and yaw, and VEL_HDG is the heading derived from GNSS velocity.
The third plot shows the sensor fusion output in the body frame at the vehicle origin after compensating for the lever arm as defined in the configuration file. In this case, you can see it is almost entirely in the x-axis which is defined as the forward direction of the vehicle with small errors in the y-axis during the GNSS outages.
The last two plots show the estimated biases for the three accelerometer axes and the three gyro axes.
For comparison, let’s disable the velocity matching option which will give us the equivalent of a true real-time solution with IMU sensor fusion, but because there is no end-of-outage correction, the errors in position and velocity will be larger.
You can see in the plot on the left below that the position estimates during the GNSS outages still roughly follow the correct trajectory, but as you can see on the right, the peak position errors have increased during the 15 second outages from around 0.5 meters to over 10 meters. In theory this can be reduced for a vehicle if we constrain the sideways and vertical motion by enabling the non-holonomic constraint option in the configuration parameters. In reality, this feature is still a little experimental and I have not been able to achieve much benefit by turning this on. I need to look into this further and hope to make it the subject of a future post.
To compare this to performance without the IMU, I will turn set the “disable_imu” option in the configuration file which eliminates use of the IMU entirely in the solution. In this case, the trajectory will coast at constant velocity during the GNSS outages. This generates the plots below, showing position peak errors during the outages of over 200 meters. The velocity matching option is still disabled, so the result is true real-time.
As a final example, I will enable the velocity matching option with the IMU still disabled. This gives the plots below showing roughly 6-12 meter peak position errors during the 15 second GNSS outages. I’ve zoomed in on the position plot to more easily see the shape of the position errors with velocity matching.
I think that should be enough to get anyone started with the code if they are interested. If you try it out, I encourage you to experiment with different Kalman filter tuning values and with various options enabled or disabled, and to dig into the code itself. With a little effort, you should also be able to adapt your own GNSS/IMU data as input, though I’ll leave a detailed discussion of that for a future post.
So, to quickly summarize these result, we see that without the IMU, in this example, we were seeing roughly 200 meter peak position errors during the 15 second GNSS outage for real-time operation. By adding the velocity matching option, and dropping back to near-real time operation, we are able to reduce this to 6-12 meter peak errors. Adding a low-cost IMU reduced these numbers to roughly 10 meter peak errors for real-time operation and 0.5 meters for near-real time operation.
I should emphasize that sensor fusion is a relatively new topic for me and I’ve learned a lot by putting together this code set, but it is still a work in progress. I’m sure I’ve gotten a few things wrong or at least non-optimal so if you see anything I’m doing incorrectly or could do better, please leave a comment below, or better yet, generate a pull request for the Github code.
rtklibexplorer 