๐ก Sensor Fusion Foundations
๐ฏ What is Sensor Fusion?
Sensor Fusion is the process of combining data from multiple sensors to produce more accurate, reliable, and complete information than any single sensor can provide. It's fundamental to autonomous vehicles, robotics, AR/VR, and IoT systems.
Core Principles
Redundancy: Multiple sensors provide fail-safe operation. Complementarity: Different sensors cover different aspects (e.g., vision + LIDAR). Timeliness: Sensor fusion combines data with different latencies.
import numpy as np
class SensorFusion:
def __init__(self):
self.measurements = []
self.confidence = []
def add_measurement(self, value, uncertainty):
self.measurements.append(value)
self.confidence.append(1.0 / uncertainty)
def fuse(self):
# Weighted average based on confidence
total_weight = sum(self.confidence)
fused = sum(m * c for m, c in zip(self.measurements, self.confidence)) / total_weight
return fused๐ฏ Kalman Filter Basics
๐ฏ Linear Gaussian Filter
Kalman filter is optimal for linear systems with Gaussian noise. It provides recursive estimation of state using prediction and update steps.
Kalman Filter Steps
Prediction: xฬโโ = Fโ xฬโโโ + Bโ uโ. Update: Kโ = Pโโ Hโแต (Hโ Pโโ Hโแต + Rโ)โปยน. Correction: xฬโ = xฬโโ + Kโ (zโ - Hโ xฬโโ).
class KalmanFilter:
def __init__(self, F, H, Q, R, x0, P0):
self.F = F # State transition
self.H = H # Observation
self.Q = Q # Process noise
self.R = R # Measurement noise
self.x = x0 # State
self.P = P0 # Covariance
def predict(self, u=0):
self.x = self.F @ self.x + u
self.P = self.F @ self.P @ self.F.T + self.Q
return self.x
def update(self, z):
y = z - self.H @ self.x
S = self.H @ self.P @ self.H.T + self.R
K = self.P @ self.H.T @ np.linalg.inv(S)
self.x = self.x + K @ y
self.P = (np.eye(len(self.x)) - K @ self.H) @ self.P
return self.x๐ Extended Kalman Filter
๐ฏ Nonlinear State Estimation
EKF linearizes nonlinear functions using Jacobians, extending Kalman filter to nonlinear systems.
EKF Approach
Linearize around current estimate: Fโ โ โf/โx | xฬโโโ, Hโ โ โh/โx | xฬโโ. The EKF is widely used in navigation and robotics.
class ExtendedKalmanFilter:
def __init__(self, f, h, F_jac, H_jac, Q, R, x0, P0):
self.f = f # Nonlinear state transition
self.h = h # Nonlinear observation
self.F_jac = F_jac # Jacobian of f
self.H_jac = H_jac # Jacobian of h
self.Q = Q
self.R = R
self.x = x0
self.P = P0
def predict(self, u=0):
self.x = self.f(self.x, u)
F = self.F_jac(self.x, u)
self.P = F @ self.P @ F.T + self.Q
return self.x
def update(self, z):
H = self.H_jac(self.x)
y = z - self.h(self.x)
S = H @ self.P @ H.T + self.R
K = self.P @ H.T @ np.linalg.inv(S)
self.x = self.x + K @ y
self.P = (np.eye(len(self.x)) - K @ H) @ self.P
return self.x๐ Unscented Kalman Filter
๐ฏ Sigma-Point Approach
UKF avoids linearization by propagating sigma points through the nonlinear function, providing better accuracy for highly nonlinear systems.
Unscented Transform
Sigma points are deterministically chosen to capture the mean and covariance. They are propagated through the nonlinear function, and the transformed statistics are computed.
class UnscentedKalmanFilter:
def __init__(self, f, h, Q, R, x0, P0, alpha=0.1, beta=2, kappa=0):
self.f = f
self.h = h
self.Q = Q
self.R = R
self.x = x0
self.P = P0
self.n = len(x0)
self.alpha = alpha
self.beta = beta
self.kappa = kappa
self.lambda_ = alpha**2 * (self.n + kappa) - self.n
def compute_sigma_points(self):
# Compute sigma points and weights
pass
def predict(self, u=0):
# UKF prediction step
pass
def update(self, z):
# UKF update step
pass๐ฒ Particle Filters
๐ฏ Monte Carlo Localization
Particle filters represent the posterior distribution using weighted particles, enabling non-Gaussian, multi-modal estimation.
Particle Filter Steps
Sampling: Draw particles from proposal distribution. Weighting: Update weights based on likelihood. Resampling: Replace low-weight particles with high-weight ones.
class ParticleFilter:
def __init__(self, n_particles, x0, process_noise):
self.n = n_particles
self.particles = np.random.normal(x0, process_noise, (n_particles, len(x0)))
self.weights = np.ones(n_particles) / n_particles
def predict(self, motion_model, u):
for i in range(self.n):
self.particles[i] = motion_model(self.particles[i], u)
def update(self, z, likelihood_model):
for i in range(self.n):
self.weights[i] = likelihood_model(z, self.particles[i])
self.weights /= np.sum(self.weights)
def resample(self):
indices = np.random.choice(self.n, self.n, p=self.weights)
self.particles = self.particles[indices]
self.weights = np.ones(self.n) / self.n
def estimate(self):
return np.average(self.particles, weights=self.weights, axis=0)๐ Bayesian Inference
๐ฏ Probabilistic Framework
Bayesian inference provides a rigorous framework for combining prior knowledge with sensor measurements to update beliefs.
Bayesian Updating
P(ฮธ|x) โ P(x|ฮธ) ยท P(ฮธ). Prior P(ฮธ) represents initial belief. Likelihood P(x|ฮธ) models sensor measurements. Posterior P(ฮธ|x) is updated belief.
def bayesian_update(prior, likelihood, measurement):
"""Update prior with new measurement using Bayes' rule"""
posterior = likelihood(measurement) * prior
posterior = posterior / np.sum(posterior)
return posterior๐ฑ Sensor Types & Models
๐ฏ Common Sensors in Fusion
GPS, IMU, cameras, LIDAR, radar, sonar, ultrasonic, and infrared sensors each have unique characteristics and error models.
Sensor Error Models
Gaussian noise: Most sensors have Gaussian measurement noise. Bias: Systematic offset that drifts over time. Outliers: Spurious measurements that need robust handling.
class SensorModel:
def __init__(self, noise_std, bias=0):
self.noise_std = noise_std
self.bias = bias
def measure(self, true_value):
return true_value + self.bias + np.random.normal(0, self.noise_std)
def likelihood(self, z, x):
return np.exp(-0.5 * ((z - x - self.bias) / self.noise_std)**2)๐ Sensor Alignment
๐ฏ Spatial & Temporal Calibration
Sensor alignment involves extrinsic calibration (rotation/translation between sensors) and temporal synchronization.
Calibration Methods
Extrinsic calibration: Estimate rotation matrix R and translation t between sensor frames. Temporal alignment: Synchronize timestamps across sensors with different clocks.
def align_sensors(sensor1_points, sensor2_points):
"""Estimate rigid transformation between two sensor frames"""
# Compute centroids
centroid1 = np.mean(sensor1_points, axis=0)
centroid2 = np.mean(sensor2_points, axis=0)
# Center points
centered1 = sensor1_points - centroid1
centered2 = sensor2_points - centroid2
# Compute rotation using SVD
H = centered1.T @ centered2
U, _, Vt = np.linalg.svd(H)
R = Vt.T @ U.T
# Compute translation
t = centroid2 - R @ centroid1
return R, t๐ GPS/INS Integration
๐ฏ Navigation System Core
GPS provides absolute position with low frequency (1-10 Hz). INS provides high-frequency (100-1000 Hz) relative motion. Fusion gives robust navigation.
GPS/INS Fusion Benefits
GPS: Accurate but can be denied (urban canyons, indoors). INS: High frequency but drifts over time. Fusion: Best of both worlds โ accurate and continuous.
class GPS_INS_Fusion:
def __init__(self):
self.kf = KalmanFilter(
F=np.eye(6),
H=np.eye(6),
Q=np.eye(6) * 0.01,
R=np.eye(6) * 0.1,
x0=np.zeros(6),
P0=np.eye(6) * 10
)
def update_ins(self, accel, gyro, dt):
# INS prediction using IMU
pass
def update_gps(self, gps_position):
# GPS update
self.kf.update(gps_position)๐ท Camera-LiDAR Fusion
๐ฏ Vision + Depth
Cameras provide rich semantic information. LiDAR provides accurate depth. Fusion enables 3D object detection and scene understanding.
Fusion Approaches
Early fusion: Combine raw data. Late fusion: Combine object detections. Intermediate: Feature-level fusion using neural networks.
class CameraLidarFusion:
def __init__(self):
self.camera = CameraModel()
self.lidar = LidarModel()
self.calibration = self.load_calibration()
def project_lidar_to_camera(self, lidar_points):
# Project LiDAR points to camera image plane
points_camera = self.calibration.R @ lidar_points + self.calibration.t
points_image = self.calibration.K @ points_camera
return points_image / points_image[:, 2:]
def fuse(self, image, lidar_points):
# LiDAR-camera fusion
projected = self.project_lidar_to_camera(lidar_points)
# Associate LiDAR points with image features
return self.associate_points(image, projected)๐ก Radar & Sonar
๐ฏ Radar and Sonar Sensing
Radar provides velocity and distance using Doppler effect. Sonar uses sound waves for underwater applications.
Radar Characteristics
Doppler: Measures radial velocity. Range: Time-of-flight measurement. Angular resolution: Limited but improving with MIMO arrays.
class RadarModel:
def __init__(self, range_noise, angle_noise, velocity_noise):
self.range_noise = range_noise
self.angle_noise = angle_noise
self.velocity_noise = velocity_noise
def measure(self, target):
range_true = np.linalg.norm(target)
angle_true = np.arctan2(target[1], target[0])
velocity_true = np.linalg.norm(target[3:5])
range_meas = range_true + np.random.normal(0, self.range_noise)
angle_meas = angle_true + np.random.normal(0, self.angle_noise)
velocity_meas = velocity_true + np.random.normal(0, self.velocity_noise)
return range_meas, angle_meas, velocity_measโก IMU & Orientation
๐ฏ Inertial Measurement Units
IMUs combine accelerometers, gyroscopes, and sometimes magnetometers to measure acceleration, angular velocity, and orientation.
Attitude Estimation
Accelerometer: Measures gravity vector (attitude). Gyroscope: Measures angular velocity (rate). Magnetometer: Measures heading (yaw). Fusion using complementary filter or Madgwick filter.
class IMU:
def __init__(self, accel_bias=0, gyro_bias=0, mag_bias=0):
self.accel_bias = accel_bias
self.gyro_bias = gyro_bias
self.mag_bias = mag_bias
def read_accel(self, gravity=9.81):
return np.random.normal([0, 0, gravity], [0.1, 0.1, 0.1]) + self.accel_bias
def read_gyro(self):
return np.random.normal([0, 0, 0], [0.01, 0.01, 0.01]) + self.gyro_bias
def read_mag(self):
return np.random.normal([1, 0, 0], [0.05, 0.05, 0.05]) + self.mag_bias๐ง Deep Learning Fusion
๐ฏ Neural Network-Based Fusion
Deep learning enables sensor fusion through end-to-end learning, attention mechanisms, and multi-modal feature extraction.
DL Fusion Architectures
Multi-modal: Separate encoders for each sensor. Attention: Learn which sensors to focus on. Transformer: Cross-attention between sensor streams.
# PyTorch-style deep sensor fusion
import torch
import torch.nn as nn
class SensorFusionNN(nn.Module):
def __init__(self):
super().__init__()
self.camera_encoder = nn.Sequential(
nn.Conv2d(3, 64, 3),
nn.ReLU(),
nn.AdaptiveAvgPool2d((1, 1))
)
self.lidar_encoder = nn.Sequential(
nn.Linear(3, 128),
nn.ReLU(),
nn.Linear(128, 64)
)
self.fusion = nn.Sequential(
nn.Linear(128, 64),
nn.ReLU(),
nn.Linear(64, 10) # Number of classes
)
def forward(self, camera_input, lidar_input):
camera_features = self.camera_encoder(camera_input)
lidar_features = self.lidar_encoder(lidar_input)
fused = torch.cat([camera_features, lidar_features], dim=1)
return self.fusion(fused)๐ Applications & Systems
๐ฏ Real-World Sensor Fusion
Autonomous vehicles, robotics, AR/VR, healthcare, industrial automation, smart cities, and more.
Application Areas
Autonomous Driving: GPS, IMU, camera, LiDAR, radar fusion for perception and localization. Robotics: SLAM, localization, mapping, object tracking. AR/VR: Head tracking, hand tracking, spatial mapping.
class AutonomousVehicle:
def __init__(self):
self.gps = GPS()
self.imu = IMU()
self.camera = Camera()
self.lidar = Lidar()
self.radar = Radar()
self.fusion_engine = SensorFusionEngine()
def perceive(self):
# Get measurements from all sensors
gps_data = self.gps.read()
imu_data = self.imu.read()
camera_data = self.camera.capture()
lidar_data = self.lidar.scan()
radar_data = self.radar.scan()
# Fuse sensor data
state = self.fusion_engine.fuse(
gps=gps_data,
imu=imu_data,
camera=camera_data,
lidar=lidar_data,
radar=radar_data
)
return state