⚡ electrical engineering

FPGA-Based Real-Time Sensor Fusion for Inertial Navigation Using IMU and GPS Data

This paper presents a low-latency, real-time FPGA implementation of an IMU/GPS Extended Kalman Filter for inertial navigation, achieved by optimizing a reduced 16-state model through High-Level Synthesis on a Xilinx Kintex-7 platform to deliver high-precision estimation with minimal resource utilization.

Original authors: Abbas Fadavi

Published 2026-08-11

📖 5 min read🧠 Deep dive

CC BY 4.0

Original authors: Abbas Fadavi

Original paper licensed under CC BY 4.0 (https://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine you are trying to find your way through a dense, foggy forest without a map. You have two tools to help you. The first is a super-fast, super-accurate pedometer and a spinning top strapped to your wrist (an IMU). These tell you exactly how fast you are moving and which way you are turning, but they are a bit "drifty." If you use them for too long, they slowly start to lie to you, making you think you're walking in a straight line when you're actually spiraling. The second tool is a satellite phone (GPS). It tells you exactly where you are on the map, but it's slow to connect and only gives you a fix once in a while. If you rely only on the satellite, you might miss a sudden turn; if you rely only on the pedometer, you'll eventually wander off the path.

To solve this, engineers use a clever math trick called a "sensor fusion" algorithm, specifically something called an Extended Kalman Filter (EKF). Think of this filter as a super-smart referee that constantly listens to both the fast pedometer and the slow satellite. It guesses where you are, checks the satellite to see if the guess was right, and then corrects the pedometer's drift. The problem is that doing this math fast enough to keep up with the pedometer's high speed is incredibly heavy lifting for a standard computer chip. This is where Field-Programmable Gate Arrays (FPGAs) come in. If a normal computer chip is a single chef cooking a meal one step at a time, an FPGA is a massive kitchen with hundreds of chefs working in perfect sync, all at the exact same time. This paper explores how to build that super-kitchen to run the navigation referee in real-time.

The Paper's Mission: Building a Super-Fast Navigation Brain

This paper by Abbas Fadavi presents a new way to build that "super-kitchen" for navigation. The author took a complex navigation algorithm that usually runs on a computer and successfully translated it into a design that lives directly on a Xilinx Kintex-7 FPGA chip. The goal was to make the "referee" (the EKF) fast enough to handle the high-speed data from the IMU while waiting for the slow GPS updates, all without getting bogged down by the heavy math.

The Challenge: From Math Class to Hardware
The journey started with a big hurdle: the original algorithm was written in MATLAB, a software tool full of fancy "toolbox" features that hardware chips can't understand. It was like trying to cook a recipe written for a high-tech molecular gastronomy lab using a campfire. The author had to strip away the fancy software dependencies and rewrite the code so it could be synthesized (translated) into hardware logic.

One major change involved the "state vector," which is just a list of all the things the computer is trying to guess about your position (like where you are, how fast you're going, and which way you're facing). The original MATLAB model tried to guess 22 different things, including magnetic field data. The author realized that for this specific GPS/IMU setup, the magnetic data wasn't strictly necessary. By cutting the list down to 16 key items, they reduced the amount of math the chip had to do by about 65%. It's like realizing you don't need to track the temperature of every single leaf in the forest to know you're lost; you just need to track your path and speed.

The Magic Tricks: How They Made it Fast
To make the math fly, the author used a few clever tricks:

Analytical Formulas: Instead of letting the computer guess and check (iterative methods) to solve complex math problems, they used exact, pre-calculated formulas. It's the difference between guessing the answer to a riddle by trying every word in the dictionary versus just knowing the answer.
Cholesky Factorization: For the heavy lifting of matrix math (which is like organizing a giant spreadsheet of probabilities), they used a specific, efficient method called Cholesky factorization. This is like using a shortcut to solve a puzzle instead of trying to fit every piece in every possible spot.
Pipelining: They used special instructions called "PIPELINE" and "ARRAY_PARTITION." Imagine a factory assembly line where every worker does one tiny step and passes the work to the next person immediately. This allows the chip to process data continuously without stopping to wait for the previous step to finish.

The Results: Speed and Accuracy
The team tested their design on a Xilinx Kintex-7 FPGA. The results were impressive. The chip ran at a clock speed of 50 MHz. Every time it needed to update its guess of your location, it took between 1.12 milliseconds and 1.21 milliseconds. That is incredibly fast—fast enough to keep up with the high-speed IMU sensors.

In terms of resources, the design was surprisingly efficient. It used only 191 "DSP slices" (the chip's math engines), which is just 12% of the total available power on the chip. This leaves plenty of room for other tasks.

Most importantly, the author compared the chip's output to the original computer model. The difference was tiny—less than 0.001% error. This proves that even though they simplified the math and changed the code to fit the hardware, the chip is still telling the truth. The paper concludes that by using High-Level Synthesis (HLS) tools and smart simplifications, it is possible to build a real-time, low-latency navigation system on a mid-range chip that is both fast and accurate.

Technical Summary: FPGA-Based Real-Time Sensor Fusion for Inertial Navigation Using IMU and GPS Data

Problem Statement
Real-time inertial navigation requires continuous state estimation under strict latency constraints. While Inertial Measurement Units (IMUs) provide high-rate motion data, they suffer from drift over time, whereas Global Positioning System (GPS) receivers offer low-rate absolute positioning. The Extended Kalman Filter (EKF) is the standard solution for fusing these complementary sensors; however, deploying EKF algorithms at IMU rates exceeding 100 Hz remains computationally demanding due to the rapid scaling of covariance propagation and matrix operations with state dimension. Furthermore, transitioning from high-level algorithmic specifications (e.g., MATLAB) to hardware implementations often faces challenges regarding toolbox dependencies, data structure constraints, and the complexity of Jacobian approximations in resource-constrained environments.

Methodology
The paper presents a systematic methodology for translating a MATLAB-based IMU/GPS EKF navigation model into a synthesizable High-Level Synthesis (HLS) C++ design. The implementation targets a Xilinx Kintex-7 (xc7k410t) FPGA using Vitis HLS 2023.1.

Key methodological adaptations include:

State Vector Reduction: The original 22-dimensional MATLAB model, which included geomagnetic and magnetometer bias states, was reduced to a 16-state architecture. This involved removing the magnetometer update stage, as GPS provides sufficient observability for the remaining states in the target scenario. This reduction decreased the covariance matrix dimension from 22×22 to 16×16.
Algorithmic Optimization:
- Jacobian Evaluation: Iterative finite-difference Jacobian evaluation was replaced with a closed-form analytical formulation to reduce computational complexity.
- Matrix Operations: Kalman gain computation utilizes Cholesky factorization instead of direct matrix inversion for improved numerical stability and lower hardware cost. The covariance update employs a simplified form rather than the full Joseph form, halving the required matrix multiplications.
- Coordinate Conversion: MATLAB's lla2ned toolbox function was replaced with a custom lla2ned_simple function based on a spherical Earth approximation, with reference-dependent constants precomputed to eliminate runtime trigonometric evaluations.
Control Flow Restructuring: To satisfy HLS requirements for statically bounded loops, the original data-dependent while loop was restructured into two nested for loops with fixed bounds (50 GPS blocks, each containing 160 IMU samples). The logic was decomposed into independent imu_predict and gps_update functions to enable independent pipelining.
Numerical Stability: Conditional branches for quaternion normalization were removed and replaced with branchless formulations (e.g., max clamping) to prevent pipeline stalls. All arrays were declared with compile-time constant dimensions.
Hardware Acceleration: The design employs HLS directives such as PIPELINE, UNROLL, and ARRAY_PARTITION to maximize parallelism and memory access efficiency.

Key Contributions
The paper identifies three primary contributions:

A systematic methodology for translating MATLAB-based EKF navigation code to HLS-C++, specifically resolving issues related to toolbox dependencies and data structure constraints.
The application of specific HLS directives to achieve high-throughput execution capable of handling 160 IMU samples per GPS update cycle.
Demonstration of a fully synthesizable implementation on a mid-range Kintex-7 FPGA that achieves real-time throughput with low latency.

Experimental Results
The implementation was validated against a MATLAB reference model using simulated IMU and GPS data.

Functional Verification: Numerical validation confirmed functional equivalence, with relative output errors for position, orientation, and covariance remaining below $10^{-3}\%$ (specifically, maximum relative errors were $<10^{-4}$ for position and orientation, and $<10^{-3}$ for the covariance matrix).
Timing Performance: Synthesized at a target clock of 50 MHz, the design achieved an operating frequency of approximately 59.4 MHz. The total estimation latency per update cycle ranged between 1.12 ms and 1.21 ms. The GPS update stage (involving matrix operations) dominated the latency (0.514–0.600 ms), while the IMU prediction stage required only 3.74 $\mu$ s.
Resource Utilization: The design utilized 191 DSP slices (12% of the device), 44,317 LUTs (17%), and 42,955 Flip-Flops (8%). The majority of resources were consumed by the GPS update module.

Significance
The paper claims significance in addressing the research gap between algorithmic specification and hardware implementation for embedded navigation. By demonstrating an HLS-based EKF that resolves state vector incompatibilities, reduces Jacobian computation complexity through structural exploitation, and achieves real-time throughput on a mid-range FPGA, the work offers a flexible and efficient alternative to manual HDL coding. The results confirm that such optimizations allow for the deployment of complex sensor fusion algorithms on hardware with modest resource footprints while maintaining numerical accuracy comparable to software reference models.

More like this