Chapter 6: Sensor Fusion

Multi-Sensor Integration

Combining data from multiple sensors for better perception.

LiDAR Processing

from sensor_msgs.msg import LaserScan

class LidarProcessor(Node):
    def __init__(self):
        super().__init__('lidar_processor')
        self.subscription = self.create_subscription(
            LaserScan,
            'scan',
            self.scan_callback,
            10
        )
    
    def scan_callback(self, msg):
        # Find closest obstacle
        min_distance = min(msg.ranges)
        min_index = msg.ranges.index(min_distance)
        angle = msg.angle_min + min_index * msg.angle_increment
        
        self.get_logger().info(
            f'Closest obstacle: {min_distance:.2f}m at {angle:.2f} rad'
        )

IMU Integration

from sensor_msgs.msg import Imu

class ImuProcessor(Node):
    def __init__(self):
        super().__init__('imu_processor')
        self.subscription = self.create_subscription(
            Imu,
            'imu/data',
            self.imu_callback,
            10
        )
    
    def imu_callback(self, msg):
        # Get orientation
        orientation = msg.orientation
        angular_velocity = msg.angular_velocity
        linear_acceleration = msg.linear_acceleration

Kalman Filter

import numpy as np

class KalmanFilter:
    def __init__(self):
        self.x = np.zeros(4)  # [x, y, vx, vy]
        self.P = np.eye(4)
        
    def predict(self, dt):
        # State transition
        F = np.array([
            [1, 0, dt, 0],
            [0, 1, 0, dt],
            [0, 0, 1, 0],
            [0, 0, 0, 1]
        ])
        self.x = F @ self.x
        self.P = F @ self.P @ F.T
    
    def update(self, z):
        # Measurement update
        H = np.array([[1, 0, 0, 0], [0, 1, 0, 0]])
        y = z - H @ self.x
        S = H @ self.P @ H.T
        K = self.P @ H.T @ np.linalg.inv(S)
        self.x = self.x + K @ y
        self.P = (np.eye(4) - K @ H) @ self.P

Next: Chapter 7 - Autonomous Behaviors!