Sensor Fusion for Robots: Combining Data for Better Perception

Every sensor lies. Accelerometers drift. Gyroscopes accumulate error. GPS jumps around. Cameras lose tracking in bad lighting. No single sensor gives you a complete, reliable picture of the world. And yet, robots need to know exactly where they are, how they are oriented, and what is around them -- in real time, without fail.

The solution is sensor fusion: combining data from multiple imperfect sensors to produce an estimate that is better than any individual sensor could provide alone. It is one of the most important techniques in robotics, and it is behind everything from drone stabilization to self-driving car navigation.

This post explains why sensor fusion matters, how the most common algorithms work, and how to implement them yourself.

Why a Single Sensor Is Never Enough

Every sensor has fundamental limitations. Understanding these limitations is the first step toward understanding why fusion is necessary.

Accelerometers measure linear acceleration, including gravity. You can use them to estimate tilt (pitch and roll) because gravity provides a constant reference vector. The problem is that any motion of the robot -- vibration, turning, speeding up -- corrupts the reading. Accelerometers are noisy on short timescales but correct on average over longer periods.

Gyroscopes measure angular velocity. Integrate that over time and you get an orientation estimate. The reading is smooth and responsive, but integration accumulates error. After a few minutes, your gyro-only estimate can be off by several degrees. This is called gyroscope drift, and it only gets worse over time.

GPS gives you a global position, but only outdoors, with meter-level accuracy at best, and at update rates of just 1-10 Hz. It is useless indoors, unreliable near buildings, and far too slow for real-time control.

Wheel encoders track how much each wheel has rotated, which lets you estimate distance traveled and heading changes. This is called odometry. It works well over short distances but accumulates error from wheel slippage, uneven terrain, and manufacturing imprecision. Over long runs, odometry drifts significantly.

LiDAR provides precise distance measurements in a 2D or 3D scan. It is excellent for obstacle detection and mapping but tells you nothing about orientation or velocity directly. It can also be confused by reflective surfaces, rain, or dust.

Cameras provide rich visual information but are sensitive to lighting changes, motion blur, and featureless environments. Extracting reliable position or orientation from images alone requires significant computation.

The key insight is that these sensors fail in complementary ways. Accelerometers are noisy but do not drift. Gyroscopes are smooth but drift over time. GPS is absolute but slow and imprecise. Wheel encoders are fast but accumulate error. Sensor fusion exploits these complementary strengths to produce an estimate that is accurate, responsive, and robust.

The Sensors You Will Encounter Most Often

In practice, the most common sensor fusion scenario in robotics involves an IMU (Inertial Measurement Unit). An IMU typically packages an accelerometer, a gyroscope, and sometimes a magnetometer into a single chip. Fusing the accelerometer and gyroscope data from an IMU to estimate orientation is the canonical sensor fusion problem, and it is where most roboticists start.

Beyond the IMU, the sensors you will fuse depend on your application:

• Mobile ground robots commonly fuse IMU + wheel encoders + LiDAR for localization and mapping.
• Drones fuse IMU + barometer + GPS + magnetometer for attitude and position estimation.
• Self-driving cars fuse IMU + GPS + LiDAR + cameras + radar for a complete picture of ego-motion and surroundings.
• Robot arms fuse joint encoders + force/torque sensors + sometimes vision for precise manipulation.

The algorithms are the same regardless of which sensors you are combining. What changes is the state vector and the measurement models.

The Complementary Filter: The Simplest Approach

The complementary filter is the easiest sensor fusion algorithm to understand and implement. It is commonly used to estimate pitch and roll from an IMU by combining accelerometer and gyroscope readings.

The idea is straightforward. The gyroscope gives you a fast, smooth, short-term orientation estimate but drifts over time. The accelerometer gives you a noisy but drift-free long-term reference. A complementary filter is just a weighted blend of the two:

angle = alpha * (angle + gyro_rate * dt) + (1 - alpha) * accel_angle

The parameter alpha (typically between 0.95 and 0.99) controls the tradeoff. A high alpha trusts the gyroscope more, giving you a smooth but potentially drifty estimate. A low alpha trusts the accelerometer more, giving you a drift-free but noisier estimate.

The term "complementary" comes from the frequency domain: the gyroscope acts as a high-pass filter (keeping fast changes) and the accelerometer acts as a low-pass filter (keeping the long-term average). Together, they cover the full frequency spectrum.

Here is a complete Python implementation:

import math
 
class ComplementaryFilter:
    def __init__(self, alpha=0.98):
        self.alpha = alpha
        self.pitch = 0.0
        self.roll = 0.0
 
    def update(self, accel_x, accel_y, accel_z, gyro_x, gyro_y, dt):
        # Orientation estimate from accelerometer (gravity reference)
        accel_pitch = math.atan2(accel_x, math.sqrt(accel_y**2 + accel_z**2))
        accel_roll = math.atan2(accel_y, math.sqrt(accel_x**2 + accel_z**2))
 
        # Integrate gyroscope rates for short-term estimate
        gyro_pitch = self.pitch + gyro_x * dt
        gyro_roll = self.roll + gyro_y * dt
 
        # Blend: trust gyro for short-term, accel for long-term
        self.pitch = self.alpha * gyro_pitch + (1 - self.alpha) * accel_pitch
        self.roll = self.alpha * gyro_roll + (1 - self.alpha) * accel_roll
 
        return self.pitch, self.roll
 
 
# Example usage: simulate a sensor loop
cf = ComplementaryFilter(alpha=0.98)
dt = 0.01  # 100 Hz update rate
 
# In a real system, these come from your IMU driver
accel_x, accel_y, accel_z = 0.1, 0.0, 9.81  # m/s^2
gyro_x, gyro_y = 0.01, 0.0                    # rad/s
 
pitch, roll = cf.update(accel_x, accel_y, accel_z, gyro_x, gyro_y, dt)
print(f"Pitch: {math.degrees(pitch):.2f} deg, Roll: {math.degrees(roll):.2f} deg")

The complementary filter is popular in hobby drones and small robots because it is trivial to implement, requires almost no computation, and works surprisingly well for attitude estimation. Its main limitation is that it has only one tuning parameter and no principled way to handle varying sensor noise or additional sensor inputs.

The Kalman Filter: Optimal Estimation Under Uncertainty

The Kalman filter is the gold standard of sensor fusion. Where the complementary filter uses a fixed blending weight, the Kalman filter dynamically adjusts how much it trusts each sensor based on a mathematical model of their uncertainties.

At its core, the Kalman filter maintains two things: a state estimate (what you think the system is doing) and an uncertainty estimate (how confident you are). It operates in a two-step cycle:

Predict step: Use your model of how the system evolves to predict the next state. Your uncertainty grows because the model is not perfect.

Update step: Incorporate a new sensor measurement. Compare what you predicted the sensor should read versus what it actually reads. The difference (called the innovation) is used to correct your estimate. Your uncertainty shrinks because you gained information.

The beauty of the Kalman filter is the Kalman gain: a matrix that optimally balances the prediction and the measurement based on their respective uncertainties. If your prediction is very uncertain but your sensor is precise, the Kalman gain is high and you trust the sensor. If your prediction is confident but the sensor is noisy, the Kalman gain is low and you mostly keep your prediction.

Here is a simplified implementation for a 1D position estimation problem (tracking position from noisy measurements):

import numpy as np
 
class KalmanFilter:
    def __init__(self, F, H, Q, R, x0, P0):
        """
        F: State transition matrix (how state evolves)
        H: Measurement matrix (how state maps to sensor readings)
        Q: Process noise covariance (model uncertainty)
        R: Measurement noise covariance (sensor uncertainty)
        x0: Initial state estimate
        P0: Initial uncertainty covariance
        """
        self.F = F
        self.H = H
        self.Q = Q
        self.R = R
        self.x = x0
        self.P = P0
 
    def predict(self):
        # Project state forward
        self.x = self.F @ self.x
        # Project uncertainty forward
        self.P = self.F @ self.P @ self.F.T + self.Q
 
    def update(self, z):
        # Compute Kalman gain
        S = self.H @ self.P @ self.H.T + self.R
        K = self.P @ self.H.T @ np.linalg.inv(S)
 
        # Correct state estimate with measurement
        innovation = z - self.H @ self.x
        self.x = self.x + K @ innovation
 
        # Update uncertainty
        I = np.eye(self.P.shape[0])
        self.P = (I - K @ self.H) @ self.P
 
        return self.x
 
 
# Example: track 1D position and velocity from position measurements
dt = 0.1
 
# State: [position, velocity]
F = np.array([[1, dt],
              [0,  1]])    # constant velocity model
 
H = np.array([[1, 0]])     # we only measure position
 
Q = np.array([[0.01, 0],
              [0, 0.01]])  # process noise
 
R = np.array([[1.0]])      # measurement noise (sensor variance)
 
x0 = np.array([[0],
               [0]])       # start at origin, zero velocity
 
P0 = np.array([[1, 0],
               [0, 1]])    # initial uncertainty
 
kf = KalmanFilter(F, H, Q, R, x0, P0)
 
# Simulate noisy measurements of an object moving at ~1 m/s
true_positions = [i * 0.1 for i in range(100)]
measurements = [p + np.random.normal(0, 1.0) for p in true_positions]
 
for z in measurements:
    kf.predict()
    estimate = kf.update(np.array([[z]]))
    # estimate[0] is filtered position, estimate[1] is estimated velocity

The key tuning parameters are the Q matrix (process noise covariance) and R matrix (measurement noise covariance). Q represents how much you expect the system to deviate from your model between timesteps. R represents how noisy your sensor is. Getting these right is critical: if Q is too small, the filter trusts the model too much and responds slowly to real changes. If R is too small, it trusts noisy measurements too much and the estimate becomes jittery.

The Extended Kalman Filter: Handling Nonlinear Systems

The standard Kalman filter assumes that both the system dynamics and the measurement model are linear. In robotics, this is almost never true. A robot turning in a circle has nonlinear dynamics. Converting GPS coordinates to a local frame involves nonlinear trigonometry. Camera projection is nonlinear.

The Extended Kalman Filter (EKF) handles this by linearizing the nonlinear functions at each timestep using their Jacobian matrices. Instead of constant matrices F and H, you have nonlinear functions f(x) and h(x), and at each step you compute the Jacobians:

Predict:
    x_pred = f(x)           # nonlinear state transition
    F_jac = df/dx            # Jacobian of f at current state
    P_pred = F_jac * P * F_jac^T + Q
 
Update:
    z_pred = h(x_pred)      # nonlinear measurement prediction
    H_jac = dh/dx            # Jacobian of h at predicted state
    K = P_pred * H_jac^T * (H_jac * P_pred * H_jac^T + R)^-1
    x = x_pred + K * (z - z_pred)
    P = (I - K * H_jac) * P_pred

The EKF is the workhorse of robot localization. Nearly every production robot that needs to estimate its pose in the world -- from warehouse robots to Mars rovers -- uses some variant of an EKF. It is used in EKF-SLAM to simultaneously estimate the robot's pose and the positions of landmarks in the environment.

The tradeoff is complexity. You need to derive the Jacobians analytically (or compute them numerically), and the linearization can fail badly if the system is highly nonlinear or if the uncertainty is large. For those cases, the Unscented Kalman Filter (UKF) and particle filters provide alternatives that do not require linearization.

Real-World Applications

Sensor fusion is not an academic exercise. It is running right now on billions of devices:

Drone attitude estimation is the classic use case. Every quadcopter uses an IMU-based sensor fusion algorithm (usually a complementary filter or an EKF) to estimate its orientation at 200-1000 Hz. Without it, the flight controller cannot keep the drone stable.

Self-driving cars fuse LiDAR point clouds, camera images, radar returns, GPS coordinates, IMU readings, and wheel odometry to build a coherent model of where the car is and what surrounds it. The fusion happens at multiple levels: low-level (IMU + GPS for ego-motion), mid-level (LiDAR + camera for object detection), and high-level (combining all tracks into a world model).

Robot localization in warehouses, hospitals, and factories typically uses an EKF or particle filter to fuse wheel odometry with LiDAR scan matching. The odometry provides smooth, high-rate motion estimates between LiDAR scans. The LiDAR scans correct the accumulated odometry drift by matching against a known map.

Surgical robots fuse joint encoder feedback with force/torque sensors and sometimes optical tracking to achieve sub-millimeter positioning accuracy during procedures.

Smartphones fuse accelerometers, gyroscopes, magnetometers, barometers, GPS, Wi-Fi signal strength, and cellular tower signals to estimate your position and orientation. The step counter, compass, and navigation all depend on sensor fusion running in the background.

Common Pitfalls and How to Avoid Them

Sensor fusion is conceptually elegant but practically tricky. Here are the issues that trip up most people:

Sensor frame misalignment. Every sensor has its own coordinate frame. The IMU might have X pointing forward, while your robot's convention has X pointing right. If you do not carefully transform all sensor data into a common reference frame before fusing, your estimates will be wrong in subtle, hard-to-debug ways. Always draw out your coordinate frames and verify the transforms with known test cases.

Time synchronization. Different sensors produce data at different rates and with different latencies. A LiDAR scan that arrives 50 ms late will cause the filter to associate it with the wrong predicted state. For simple systems, timestamping and interpolation may suffice. For high-performance systems, you need proper time-delay compensation in the filter.

Tuning Q and R matrices. This is where most people spend the most time. If you have datasheets for your sensors, the measurement noise variance R can often be read directly (or measured from static data). Process noise Q is harder -- it represents your model's imperfection, which is difficult to quantify. A practical approach is to start with Q and R estimated from sensor specs, then adjust based on filter performance: if the estimate is too sluggish, increase Q; if it is too jittery, increase R.

Forgetting to normalize. If you are estimating orientation using quaternions or angles, remember that angles wrap around (360 degrees equals 0 degrees) and quaternions must be unit length. Failing to handle these constraints in the innovation step causes the filter to produce nonsensical corrections when angles cross the wrapping boundary.

Overcomplicating the state vector. Start with the minimum state you need. Estimate position and velocity before adding acceleration. Estimate orientation before adding angular velocity. Every additional state dimension increases the computational cost quadratically (because of the covariance matrix) and introduces more parameters to tune.

Choosing the Right Algorithm

With several options available, how do you decide which sensor fusion algorithm to use?

Use a complementary filter if you are fusing just two sensors (typically accelerometer and gyroscope), need minimal computation, and do not require optimal performance. It is perfect for hobby projects, small embedded systems, and getting a working prototype quickly.

Use a Kalman filter if your system dynamics and measurement models are linear, you want optimal estimation, and you can characterize your noise statistics. This applies to tracking problems with constant-velocity or constant-acceleration models.

Use an Extended Kalman Filter if your system is nonlinear (which it usually is in robotics), you need to fuse multiple sensor types, and you want a principled framework for handling uncertainty. This is the default choice for most production robots.

Use a particle filter if your system is highly nonlinear, the state distribution is non-Gaussian (e.g., multi-modal -- the robot could be in several places), or you want to avoid computing Jacobians. Particle filters are common in mobile robot localization (Monte Carlo Localization) and global localization where the initial position is unknown.

Try It Yourself

Head to our Sensors and Perception module to work through the sensor fusion lessons hands-on. You will implement a complementary filter from scratch, then build up to a Kalman filter, and finally use an EKF to localize a simulated robot that fuses odometry and LiDAR. Each lesson includes a live coding environment where you can experiment with different sensor noise levels, fusion parameters, and failure scenarios.

There is no substitute for watching a Kalman filter converge in real time and developing intuition for how the Q and R matrices shape its behavior. Start with the complementary filter lesson -- it takes about fifteen minutes -- and work your way up from there.