[Update 12/18/25: I found a couple more small errors in the GNSS_IMU code since publishing this and so am updating this post with the results from the latest code. The results between the two code sets are now nearly identical for this mid-range IMU dataset.]
In my previous post, I introduced GNSS_IMU, a Python tool I’ve recently written for demonstrating loosely coupled GNSS/IMU sensor fusion based on Paul Groves’ textbook and sample Matlab code. Shortly after publishing that post, I discovered a related project from Wuhan University. It is also a demonstration of loosely coupled GNSS/IMU sensor fusion shared on GitHub. The project is called KF-GINS and has two repositories, one written in C and one in Matlab. They recently published a paper in the GPS Solutions journal introducing the project, for which they have also shared a publicly accessible copy in their GitHub repo.
It appears to be well-written code and includes a detailed document describing the theory behind the implementation, along with a variety of other supporting documents, some in English and some in Chinese. It is intended for use with IMUs ranging from consumer-grade all the way to navigation-grade, and hence has significantly greater mathematical complexity than the GNSS_IMU code, which was intended to be more lightweight and focused on consumer-grade IMUs. I definitely recommend their libraries as a great resource for anyone interested in this topic.
In addition to the code and documentation, the Matlab library includes three sample datasets, two of which use the ADIS16465 IMU from Analog Devices, a mid-range MEMS IMU. What is especially useful about these datasets is that they also include ground truth from a high-end INS system. For comparison, the ADIS16465 is available on DigiKey for about $621, over 100 times the cost of the TDK ICM45686 IMU I used during development of my code.
While the GNSS_IMU code wasn’t necessarily intended for use with this class of IMU, I thought it would be an interesting exercise to compare how it performed versus the more capable and more complex KF-GINS code, and that using one of these datasets would be a good way to make this comparison. In particular, their Dataset 2 seemed like a good choice, since it includes not just missing GNSS data, but also a mix of poor-quality GNSS data in the gaps. This is more realistic than simply creating pseudo-gaps by deleting intervals of good GNSS data, but it is also more difficult to evaluate without a high-end INS system to generate ground truth.
To make the comparison more useful, I added support files to the GNSS_IMU repo so that anyone can run it themselves, either in its original form or after making any desired changes to the input parameters or code. If you are more interested in the final results than in the details of how it was done and how to do it yourself, you can skip to the end of the post.
Before jumping into the comparison, it’s probably worth a very brief discussion of what is the same and what is different between the two codebases. Both use an extended error-state Kalman filter with states for position, velocity, attitude, gyro biases, and accelerometer biases. KF-GINS also includes states for gyro and accelerometer scale factors; in GNSS_IMU, these states are optional. In GNSS_IMU, the position and velocity states are in the ECEF frame, while in KF-GINS they are in the NED frame. For attitude calculations, GNSS_IMU uses coordinate transformation matrices, while KF-GINS primarily uses quaternions. KF-GINS includes corrections for sculling and coning and uses trapezoidal integration in the INS mechanization; GNSS_IMU does not include these corrections. Also worth mentioning is that KF_GINS has a GPL3 license which has some restrictions on commercial use, whereas the GNSS_IMU uses the less restrictive BSD3 license.
Let’s start with some details about the dataset. Below is a plot from RTKPLOT of the RTK GNSS data from this dataset. Note that the data file does not include fix status, so to better illustrate the quality of the data, I created pseudo fix statuses based on the 3-axis position sigmas. I arbitrarily set points with sigmas less than 0.05 m to fixed, less than 0.25 m to float, and everything else to single. In the plot, green indicates fix, yellow indicates float, and red indicates single. The sigmas are shown with error bars, which are only large enough to be visible on the up/down axis. Note that there is a roughly 75-second GNSS outage starting at 14:24, with only a few low-quality positions in the gap.

The GNSS_IMU code uses RTKLIB position files for GNSS input and CSV files for IMU input with a slightly different format than the KF-GINS code, so I first created a Python script to convert the KF-GINS data files to GNSS_IMU-compatible files. This script is in the src folder and is named convert_KF_GINS_files.py. Before running it, I downloaded the dataset2 folder from the KF-GINS-Matlab repo and renamed it KF_GINS. This folder contains one file with GNSS position and velocity data, one file with IMU gyro and accelerometer data, and one file with ground-truth position, velocity, and attitude data. Running the script generates GNSS_IMU-compatible input files and saves them to the same folder.
Next, I created a config file for this data called ADIS16465_config.py. I used configuration parameters identical to those used by KF-GINS in order to ensure an apples-to-apples comparison. This was made a little more difficult because the KF-GINS code uses random-walk–based IMU parameters, while the GNSS_IMU code uses power spectral density (PSD)–based parameters. Rather than trying to directly convert between the two, I added an option to GNSS_IMU to allow the parameters to be specified in either format.
My original code only output positions and velocities in the GNSS frame, since I was using GNSS as my ground truth. Because the KF-GINS ground truth data is referenced to the IMU frame, I added a configuration option to output position and velocity in either the IMU frame or the GNSS frame. These will differ due to the lever arm between them. I also turned off the zero-velocity update, velocity matching, and yaw-alignment options in GNSS_IMU, since KF-GINS does not include these. KF-GINS uses the ground-truth position, velocity, and attitude to initialize the Kalman filter. GNSS_IMU uses the initial GNSS measurement for position and velocity initialization, but it does have the option to specify ground-truth attitude, which I did. KF-GINS always includes accelerometer and gyro scale factor states in the Kalman filter. GNSS_IMU also supports these states, but I left them disabled since I generally prefer to run without the additional complexity, and enabling them had a negligible effect on the results.
Next, I added a few lines to the header of GNSS_IMU.py, the main Python file, to point to the KF-GINS data folder, and then ran it. This generated a solution file containing positions, velocities, and attitudes.
The next step was to download the full KF-GINS Matlab library and run the solution on the dataset2 data. I don’t have a recent version of Matlab, but with a few minor modifications to the input/output code, I was able to run it using Octave, the open-source Matlab alternative. I found that it ran very slowly under Windows, but significantly faster when run under Linux using WSL (Windows Subsystem for Linux). Because of its additional complexity, it was still noticeably slower than GNSS_IMU, although it’s difficult to compare directly since one is running in Octave under Linux and the other in Python under Windows.
At this point, I had a GNSS_IMU solution file, a KF-GINS solution file, and a ground-truth file, all in slightly different formats. The next step was to generate a Python script to plot the errors of the two solutions relative to the ground truth. This script is named KF_GINS_compare.py and is also located in the src folder.
The initial results showed larger errors for GNSS_IMU than for KF-GINS, which was not entirely unexpected given that the code is relatively new and had not been subject to this level of scrutiny before. After digging through the results and the code, I did not find any major issues, but I did find several small errors that, when combined, were large enough to explain most of the discrepancies. Having both the ground truth and the KF-GINS results was extremely valuable during this debugging process. I tried to be careful to only fix actual errors, not to tune parameters or make changes that would artificially improve performance on this specific dataset, and I believe I was successful in doing so. All of these fixes have been added to the repo in recent commits.
OK, so finally we get to the actual data comparison. Below are the results from the first run after fixing the bugs in the code. In this case, I made every effort to ensure that the conditions were identical for both solutions. In each figure, the GNSS_IMU solution errors are shown on top, the KF-GINS solution errors are shown below, and for position and velocity, the bottom plot also shows the GNSS measurement errors. The orientation plots show roll, pitch, and yaw errors, while the position and velocity plots show north, east, and down errors. Overall, the GNSS_IMU errors are nearly identical to the KF_GINS errors. This is quite reassuring, given that the actual code in each code set is quite different.



Now, let’s do a little experimenting with some of the additional options in GNSS_IMU. One of the additional options is to allow float and single measurements to be weighted differently than from fix measurements since I have found in the past that the reported sigmas for fixed measurements are not only smaller, but also more reliable than those for float and single measurements. Specifically, the options allow different multipliers to be applied to the measurement sigmas for float and single measurements. Since KF_GINS does not have this option, I initially set both multipliers to 1. However, in my other sample configuration files, I typically use a float sigma multiplier of 2 and a single sigma multiplier of 5, and I consider these to be reasonable default values based on my limited experience with this code.
Below are the results after changing these two values. Note that in this case, as I mentioned previously, this data set does not include fix-statuses, so I am using the pseudo fix-statuses I derived from the position sigmas. The improvement in results is quite noticeable. The position and velocity peak errors are now less than half of the previous results. I was surprised that such a seemingly minor change could make such a large effect on the results.



Let’s try one more change. In my previous post, I described the vel_match option, which uses the first GNSS position after a GNSS outage to immediately adjust the position and velocity trajectory during the outage. This is obviously not useful for true real-time applications, but in my current application—plotting mapping data on an instrument display in near real time—it is useful to have this correction applied as soon as the outage ends. Setting the float and single sigma multipliers mentioned above to zero will ignore all float and single measurements and correct the outage between points with fixed status. Below are the results with this configuration. This change has only a very small effect on orientation, so I only show the position and velocity errors. Peak position errors are reduced from the previous result by roughly another factor of two.


OK, that’s probably enough—or maybe more than enough—plots for now. As usual with my posts, the goal isn’t to do a fully comprehensive comparison or exhaustive analysis, but rather to provide a relatively quick overview. For me, this exercise provided a useful validation of the GNSS_IMU code under conditions more challenging than it was originally designed for. Being able to compare results directly with both the ground-truth data and the KF-GINS solution was extremely helpful for debugging the code. It also demonstrates that a relatively lightweight GNSS/IMU implementation can perform surprisingly well when compared with a more complex codebase when not working with navigation-grade IMUs.



































