Real-Time Sensor Fusion in Verilog: Fusing IMUs and LiDAR on FPGAs
Robotics Perception · FPGA · Verilog
Designing Real-Time Sensor Fusion Engines in Verilog: Fusing IMUs with High-Frequency LiDAR
Illustration: hardware-level sensor fusion pipeline on an FPGA
In the world of autonomous systems, spatial awareness is a race against the clock. For a robot navigating a chaotic environment — whether it is a drone maneuvering through dense tree canopies or a humanoid robot executing agile balance corrections — the perception stack must ingest, process, and merge data from completely different sensor modalities with minimal delay.
This requirement brings us to Sensor Fusion, specifically combining an Inertial Measurement Unit (IMU) with a High-Frequency LiDAR.
- The IMU acts as the robot's inner ear, providing high-frequency, low-latency state estimations (typically 1 kHz to 4 kHz) but suffering from integration drift over time.
- The LiDAR acts as the eyes, providing low-frequency, millimeter-accurate 3D point cloud maps (typically 10 Hz to 50 Hz) to anchor the system.
Fusing these asynchronous data streams is a complex engineering challenge. If you rely on a traditional CPU to align these packets, you have already lost the latency race. Here's why hardware-level architecture is changing the game, and how to implement a real-time sensor fusion engine directly on an FPGA routing fabric.
The Latency Trap: Why CPUs Fail at High-Frequency Fusion
In a traditional software-defined robotics stack, sensors are treated as peripheral devices connected via USB, PCIe, or Ethernet. When the IMU finishes a sample or the LiDAR transmits a packet, it triggers a hardware interrupt. The operating system — even a real-time kernel like RT-Linux — must pause its current execution context, handle the interrupt, copy the data payload into memory, and assign a software timestamp.
This introduces critical points of failure:
OS Scheduling Jitter
CPUs are sequential, context-switching engines. The delay between a physical sensor event and its software timestamp is non-deterministic and can fluctuate by hundreds of microseconds.
Temporal Misalignment
A LiDAR point cloud captured at t0 arrives at the host significantly later than an IMU packet from the same instant, causing math divergence in your state estimator.
Thread Contention
Networking stacks, logging, and other kernel threads compete for the same CPU cycles your fusion algorithm needs, adding unpredictable delay under load.
Power/Thermal Throttling
On embedded ARM hosts, thermal throttling under sustained load silently degrades your "real-time" guarantees exactly when the robot is working hardest.
The FPGA Alternative: Spatial Parallelism and Nanosecond Time-Stamping
Field Programmable Gate Arrays (FPGAs) eliminate scheduling jitter by replacing sequential software execution with spatial parallelism. Instead of a single core juggling tasks over time, an FPGA instantiates dedicated, independent hardware circuits for each individual sensor stream.
Sensor cores feed a dual-clock FIFO aligner, which outputs a unified AXI4-Stream
[ Physical IMU ] --> [ Verilog IMU Core ] --> [ Asynchronous ] --> [ Unified AXI ] --> Host
[ Packet ] [ Stream Bus ] System
[ Physical LiDAR ] --> [ Verilog LiDAR Core ] --> [ Aligner FIFO ] --> [ (AXI4-Stream) ]
^
[ 64-Bit Global Timer ]By connecting the hardware communication pins (SPI, I2C, or custom GMII Ethernet) directly to the FPGA's Programmable Logic (PL), you can capture raw sensor data bits at line-rate and execute Hardware Time-Stamping at the exact nanosecond a data packet's first bit crosses the physical pin boundary — completely bypassing the operating system layer.
Hardware Architecture for Asynchronous Packet Alignment
To align a high-frequency stream (IMU) with a high-volume, lower-frequency stream (LiDAR) before sending the integrated payload to a host system (an ARM processor or downstream GPU), the routing fabric follows a three-stage pipeline:
1. Dedicated Sensor Interface Cores
The FPGA instantiates parallel state machines as hardware drivers. For the IMU, a Verilog core constantly polls the sensor over SPI. For the LiDAR, a hardware MAC core unwraps incoming UDP packets from an industrial Ethernet interface.
2. Precise Hardware Time-Stamping
A global, free-running 64-bit master timer core serves as the single source of truth across the entire chip: the timestamp equals the clock cycle count multiplied by the clock period (e.g., 4 ns for a 250 MHz fabric). The moment a sensor core detects an incoming packet, it locks the current master timer value into a metadata field attached to the packet payload.
3. The Asynchronous FIFO Packet Aligner
Because sensors run at different clock speeds, their data blocks are routed into Dual-Clock Asynchronous FIFOs, safely passing data from independent sensor clock domains into a unified system clock domain. A hardware monitoring controller reads timestamps from the fronts of both FIFOs, matches them within a configured tolerance envelope, and packages them into a single AXI4-Stream frame.
Verilog Implementation Part 1: IMU Hardware Time-Stamper
This module monitors an IMU's physical "Data Ready" pin (imu_drdy), captures the 64-bit global master timer on the rising edge, and transfers the time-stamped packet into a system register buffer.
module imu_hardware_timestamper (
input wire clk, // FPGA System Clock
input wire rst_n, // Active-Low Reset
input wire imu_drdy, // Physical Data Ready Pin from IMU
input wire [63:0] global_timer, // 64-Bit Free-Running Master Clock Timer
input wire [15:0] imu_raw_data, // Data read from SPI interface
output reg [63:0] stamped_time, // Captured timestamp output
output reg [15:0] buffered_data, // Buffered sensor data output
output reg data_valid // Handshake signal for downstream FIFO
);
reg imu_drdy_r1, imu_drdy_r2;
wire drdy_rising_edge;
// Double-flop the asynchronous interrupt pin to prevent metastability
always @(posedge clk or negedge rst_n) begin
if (!rst_n) begin
imu_drdy_r1 <= 1'b0;
imu_drdy_r2 <= 1'b0;
end else begin
imu_drdy_r1 <= imu_drdy;
imu_drdy_r2 <= imu_drdy_r1;
end
end
assign drdy_rising_edge = imu_drdy_r1 && !imu_drdy_r2;
always @(posedge clk or negedge rst_n) begin
if (!rst_n) begin
stamped_time <= 64'd0;
buffered_data <= 16'd0;
data_valid <= 1'b0;
end else begin
if (drdy_rising_edge) begin
stamped_time <= global_timer;
buffered_data <= imu_raw_data;
data_valid <= 1'b1;
end else begin
data_valid <= 1'b0;
end
end
end
endmoduleVerilog Implementation Part 2: Dual-Clock FIFO Packet Aligner
This is the piece often described but rarely shown: a dual-clock asynchronous FIFO that safely crosses the IMU and LiDAR data into the shared system clock domain, using Gray-coded pointers to avoid metastability across the boundary.
module async_fifo_aligner #(
parameter DATA_WIDTH = 80, // 64-bit timestamp + 16-bit payload
parameter ADDR_WIDTH = 4 // 16-deep FIFO
) (
// Write side -- sensor clock domain
input wire wr_clk,
input wire wr_rst_n,
input wire wr_en,
input wire [DATA_WIDTH-1:0] wr_data,
output wire full,
// Read side -- unified system clock domain
input wire rd_clk,
input wire rd_rst_n,
input wire rd_en,
output reg [DATA_WIDTH-1:0] rd_data,
output wire empty
);
reg [ADDR_WIDTH:0] wr_ptr_bin, wr_ptr_gray, wr_ptr_gray_sync1, wr_ptr_gray_sync2;
reg [ADDR_WIDTH:0] rd_ptr_bin, rd_ptr_gray, rd_ptr_gray_sync1, rd_ptr_gray_sync2;
reg [DATA_WIDTH-1:0] mem [0:(1<<ADDR_WIDTH)-1];
// --- Write domain: binary -> Gray pointer, push data ---
always @(posedge wr_clk or negedge wr_rst_n) begin
if (!wr_rst_n) begin
wr_ptr_bin <= 0;
end else if (wr_en && !full) begin
mem[wr_ptr_bin[ADDR_WIDTH-1:0]] <= wr_data;
wr_ptr_bin <= wr_ptr_bin + 1'b1;
end
end
always @(*) wr_ptr_gray = (wr_ptr_bin >> 1) ^ wr_ptr_bin;
// Synchronize write pointer into the read clock domain (2-flop sync)
always @(posedge rd_clk or negedge rd_rst_n) begin
if (!rd_rst_n) begin
wr_ptr_gray_sync1 <= 0;
wr_ptr_gray_sync2 <= 0;
end else begin
wr_ptr_gray_sync1 <= wr_ptr_gray;
wr_ptr_gray_sync2 <= wr_ptr_gray_sync1;
end
end
// --- Read domain: binary -> Gray pointer, pop data ---
always @(posedge rd_clk or negedge rd_rst_n) begin
if (!rd_rst_n) begin
rd_ptr_bin <= 0;
rd_data <= 0;
end else if (rd_en && !empty) begin
rd_data <= mem[rd_ptr_bin[ADDR_WIDTH-1:0]];
rd_ptr_bin <= rd_ptr_bin + 1'b1;
end
end
always @(*) rd_ptr_gray = (rd_ptr_bin >> 1) ^ rd_ptr_bin;
// Synchronize read pointer into the write clock domain
always @(posedge wr_clk or negedge wr_rst_n) begin
if (!wr_rst_n) begin
rd_ptr_gray_sync1 <= 0;
rd_ptr_gray_sync2 <= 0;
end else begin
rd_ptr_gray_sync1 <= rd_ptr_gray;
rd_ptr_gray_sync2 <= rd_ptr_gray_sync1;
end
end
assign full = (wr_ptr_gray == {~rd_ptr_gray_sync2[ADDR_WIDTH:ADDR_WIDTH-1], rd_ptr_gray_sync2[ADDR_WIDTH-2:0]});
assign empty = (rd_ptr_gray == wr_ptr_gray_sync2);
endmoduleVerilog Implementation Part 3: Top-Level Fusion Wrapper
Tying it together — instantiate both sensor cores, the shared master timer, and the aligner FIFO, then hand off a matched pair to your AXI4-Stream output:
module sensor_fusion_top (
input wire clk,
input wire rst_n,
input wire imu_drdy,
input wire [15:0] imu_raw_data,
input wire lidar_pkt_valid,
input wire [15:0] lidar_raw_data,
output wire [79:0] fused_axis_tdata,
output wire fused_axis_tvalid,
input wire fused_axis_tready
);
reg [63:0] global_timer;
always @(posedge clk or negedge rst_n)
if (!rst_n) global_timer <= 64'd0;
else global_timer <= global_timer + 1'b1;
wire [63:0] imu_ts;
wire [15:0] imu_data;
wire imu_valid;
imu_hardware_timestamper u_imu (
.clk(clk), .rst_n(rst_n), .imu_drdy(imu_drdy),
.global_timer(global_timer), .imu_raw_data(imu_raw_data),
.stamped_time(imu_ts), .buffered_data(imu_data), .data_valid(imu_valid)
);
async_fifo_aligner #(.DATA_WIDTH(80), .ADDR_WIDTH(4)) u_aligner (
.wr_clk(clk), .wr_rst_n(rst_n),
.wr_en(imu_valid), .wr_data({imu_ts, imu_data}), .full(),
.rd_clk(clk), .rd_rst_n(rst_n),
.rd_en(fused_axis_tready), .rd_data(fused_axis_tdata), .empty(fused_axis_tvalid_n)
);
assign fused_axis_tvalid = ~fused_axis_tvalid_n;
endmodule(This wrapper shows the IMU path fully wired; mirror the same imu_hardware_timestamper pattern for a lidar_hardware_timestamper feeding a second aligner FIFO, then merge both streams with a small arbiter — that arbitration logic is a good exercise, or a topic for a future post if there's interest.)
Bridging to the Host System: Downstream State Estimation
Once packets are aligned and time-stamped in FPGA logic, they're pushed across a high-bandwidth DMA channel via AXI into the host processing ecosystem — an ARM Processing System on a Xilinx Zynq or a Kria SOM module.
Because the host receives a packet already aligned, sorted, and paired with nanosecond accuracy, the localization algorithm doesn't waste CPU cycles sorting out asynchronous delays. The Extended Kalman Filter or graph optimization engine simply reads the pre-formatted data block and calculates state estimation updates immediately, dropping tracking loop latency to microsecond thresholds.
CPU vs. FPGA: Latency at a Glance
| Metric | Software (RT-Linux, CPU) | Hardware (FPGA, this design) |
|---|---|---|
| Timestamp jitter | ±100–500 µs | Sub-nanosecond (clock-cycle bound) |
| IMU→LiDAR alignment method | Software interpolation | Hardware FIFO + tolerance window |
| Deterministic under CPU load spikes | ✗ No | ✓ Yes — dedicated silicon |
| Multi-sensor scaling | Limited by core count/threads | Parallel — add cores, not threads |
| Development complexity | Lower | Higher (HDL, timing closure) |
Best Practices at a Glance
- Always double-flop asynchronous inputs — any signal crossing from a sensor's clock domain into your FPGA fabric needs at least two flip-flops to avoid metastability, as shown in the timestamper module above.
- Use Gray-coded pointers for async FIFOs — binary pointers will bite you intermittently on real hardware, never in simulation.
- Keep the master timer free-running — never gate or reset it during normal operation, or every downstream timestamp becomes meaningless.
- Budget your tolerance window carefully — too tight and you drop valid pairs under jitter; too loose and you fuse temporally mismatched data.
- Validate on real hardware, not just simulation — clock-domain-crossing bugs are notorious for passing Icarus Verilog/GTKWave simulation cleanly while failing on silicon.
Conclusion: Building the Foundation for Physical AI
Transitioning your robotics perception pipelines from software algorithms down to explicit hardware circuits eliminates the processing bottlenecks that limit traditional multi-sensor development. By using Verilog to build hardware time-stamping engines and asynchronous FIFO architectures, you enable clean data synchronization directly on the silicon fabric — giving your robots the deterministic reflexes needed to navigate the real world safely.
Continue the FPGA Series
Ready to go from Verilog to a real bitstream? Start with our Yosys synthesis guide → or jump to place and route with NextPNR →
Disclosure: This post contains affiliate links. If you make a purchase through them, AppliedKaos may earn a small commission at no extra cost to you. All recommendations are based on genuine use and opinion.
Comments
Post a Comment