Kalman Filter cho Robot: Sensor Fusion thực hành

Vấn đề: Tại sao một sensor không đủ?

Mỗi sensor trên robot đều có hạn chế riêng:

• Wheel encoder: Chính xác ở short-term, nhưng drift theo thời gian do wheel slip, uneven terrain
• IMU (Inertial Measurement Unit): Đo acceleration và angular velocity chính xác, nhưng double integration gây drift rất nhanh (cm → m sau vài phút)
• GPS: Absolute position, nhưng noisy (sai số 2-5m outdoor), không dùng được indoor, update rate thấp (1-10 Hz)
• LiDAR SLAM: Chính xác, nhưng tốn tài nguyên tính toán và dễ fail ở featureless environments

Kalman Filter giải quyết bằng cách kết hợp tất cả sensors, lấy ưu điểm của mỗi sensor và bù đắp nhược điểm. Kết quả: ước lượng vị trí chính xác hơn bất kỳ sensor đơn lẻ nào.

Kalman Filter -- Trực giác

Hãy tưởng tượng bạn đang ước lượng vị trí một robot di chuyển trong hành lang:

1. Predict: "Dựa trên vận tốc hiện tại, mình nghĩ robot sẽ ở vị trí X sau 0.1 giây" (dùng model)
2. Update: "Sensor GPS nói robot ở vị trí Y" (dùng measurement)
3. Fuse: "Model nói X, sensor nói Y. Model tốt hơn GPS ở short-term, nên mình tin model 70%, GPS 30% → vị trí ước lượng Z"

Kalman Filter tự động tính tỷ lệ tin tưởng (Kalman Gain) dựa trên uncertainty (covariance) của mỗi nguồn thông tin. Sensor nào ít noise hơn → được "tin" nhiều hơn.

Toán học Kalman Filter

State vector

Với robot 2D, state vector thường gồm:

x = [x, y, theta, v, omega]^T

• (x, y): vị trí
• theta: heading angle
• v: linear velocity
• omega: angular velocity

Predict step

x_predicted = F * x_prev + B * u
P_predicted = F * P_prev * F^T + Q

• F: State transition matrix (mô hình chuyển động)
• B: Control input matrix
• u: Control input (ví dụ: motor commands)
• P: State covariance matrix (uncertainty)
• Q: Process noise covariance (mô hình không hoàn hảo)

Update step

y = z - H * x_predicted          # Innovation (measurement residual)
S = H * P_predicted * H^T + R    # Innovation covariance
K = P_predicted * H^T * S^(-1)   # Kalman Gain
x_updated = x_predicted + K * y  # Updated state
P_updated = (I - K * H) * P_predicted  # Updated covariance

• z: Measurement vector
• H: Observation matrix (map state → measurement space)
• R: Measurement noise covariance (sensor noise)
• K: Kalman Gain -- "tỷ lệ tin tưởng" sensor vs model

Ý nghĩa Kalman Gain

• K gần 0: Tin model hơn sensor (sensor rất noisy, hoặc model rất tốt)
• K gần 1: Tin sensor hơn model (model uncertainty cao, sensor chính xác)

Python Implementation -- Linear Kalman Filter

import numpy as np
import matplotlib.pyplot as plt

class KalmanFilter:
    """Linear Kalman Filter for robot localization."""

    def __init__(self, dim_state: int, dim_measurement: int, dim_control: int = 0):
        self.n = dim_state
        self.m = dim_measurement

        # State estimate and covariance
        self.x = np.zeros((self.n, 1))         # State vector
        self.P = np.eye(self.n) * 1000.0       # High initial uncertainty

        # System matrices
        self.F = np.eye(self.n)                # State transition
        self.H = np.zeros((self.m, self.n))    # Observation
        self.Q = np.eye(self.n) * 0.01         # Process noise
        self.R = np.eye(self.m) * 1.0          # Measurement noise
        self.B = np.zeros((self.n, dim_control)) if dim_control > 0 else None

    def predict(self, u: np.ndarray = None):
        """Predict step."""
        if u is not None and self.B is not None:
            self.x = self.F @ self.x + self.B @ u
        else:
            self.x = self.F @ self.x
        self.P = self.F @ self.P @ self.F.T + self.Q
        return self.x.copy()

    def update(self, z: np.ndarray):
        """Update step with measurement."""
        z = z.reshape(-1, 1)
        y = z - self.H @ self.x                      # Innovation
        S = self.H @ self.P @ self.H.T + self.R      # Innovation covariance
        K = self.P @ self.H.T @ np.linalg.inv(S)     # Kalman Gain

        self.x = self.x + K @ y
        I = np.eye(self.n)
        self.P = (I - K @ self.H) @ self.P
        return self.x.copy()

    def get_state(self) -> np.ndarray:
        return self.x.flatten()

    def get_covariance(self) -> np.ndarray:
        return self.P.copy()

Ví dụ: Robot 1D với encoder + GPS

# Mô phỏng robot di chuyển trên đường thẳng
np.random.seed(42)
dt = 0.1          # 10 Hz
duration = 30.0   # 30 seconds
velocity = 1.0    # 1 m/s constant velocity

# Ground truth
times = np.arange(0, duration, dt)
true_positions = velocity * times

# Noisy measurements
encoder_noise_std = 0.1   # Encoder: low noise, but drifts
gps_noise_std = 3.0       # GPS: high noise, no drift
gps_rate = 1.0            # GPS at 1 Hz

encoder_readings = true_positions + np.cumsum(
    np.random.normal(0, encoder_noise_std * dt, len(times))
)
gps_readings = true_positions + np.random.normal(0, gps_noise_std, len(times))

# Kalman Filter setup
kf = KalmanFilter(dim_state=2, dim_measurement=1)

# State: [position, velocity]
kf.F = np.array([[1, dt],
                  [0, 1]])    # Constant velocity model
kf.H = np.array([[1, 0]])    # We measure position only
kf.Q = np.array([[0.01, 0],
                  [0, 0.01]]) # Process noise
kf.R = np.array([[gps_noise_std**2]])  # GPS measurement noise
kf.x = np.array([[0], [velocity]])     # Initial state

# Run filter
kf_positions = []
kf_velocities = []
uncertainties = []

for i, t in enumerate(times):
    # Predict (every step)
    kf.predict()

    # Update with GPS (every 1 second = every 10 steps)
    if i % int(gps_rate / dt) == 0:
        kf.update(np.array([gps_readings[i]]))

    state = kf.get_state()
    kf_positions.append(state[0])
    kf_velocities.append(state[1])
    uncertainties.append(np.sqrt(kf.P[0, 0]))

# Plot results
fig, axes = plt.subplots(3, 1, figsize=(12, 10))

axes[0].plot(times, true_positions, 'g-', label='Ground Truth', linewidth=2)
axes[0].plot(times, encoder_readings, 'b.', label='Encoder (drift)', alpha=0.3, markersize=2)
axes[0].scatter(times[::10], gps_readings[::10], c='r', s=20, label='GPS (noisy)', alpha=0.5)
axes[0].plot(times, kf_positions, 'k-', label='Kalman Filter', linewidth=1.5)
axes[0].set_ylabel('Position (m)')
axes[0].set_title('Sensor Fusion: Encoder + GPS')
axes[0].legend()

axes[1].plot(times, np.array(kf_positions) - true_positions, 'k-', label='KF Error')
axes[1].fill_between(times,
    -2 * np.array(uncertainties),
    2 * np.array(uncertainties),
    alpha=0.2, color='blue', label='2-sigma uncertainty')
axes[1].set_ylabel('Error (m)')
axes[1].set_title('Estimation Error with Uncertainty Bounds')
axes[1].legend()

axes[2].plot(times, kf_velocities, 'k-', label='Estimated Velocity')
axes[2].axhline(y=velocity, color='g', linestyle='--', label='True Velocity')
axes[2].set_ylabel('Velocity (m/s)')
axes[2].set_xlabel('Time (s)')
axes[2].set_title('Velocity Estimation')
axes[2].legend()

plt.tight_layout()
plt.savefig('kalman_filter_demo.png', dpi=150)
plt.show()

Extended Kalman Filter (EKF) cho hệ phi tuyến

Linear Kalman Filter giả sử hệ thống là tuyến tính (linear). Nhưng robot thực tế không tuyến tính -- quay heading angle theta làm cho x, y thay đổi theo sin/cos.

EKF xử lý bằng cách linearize (tuyến tính hóa) tại operating point, thay F bằng Jacobian matrix.

EKF cho differential-drive robot

class ExtendedKalmanFilter:
    """EKF for differential-drive robot localization."""

    def __init__(self):
        # State: [x, y, theta]
        self.x = np.zeros((3, 1))
        self.P = np.eye(3) * 0.1

        # Noise
        self.Q = np.diag([0.1, 0.1, 0.05])   # Process noise
        self.R_gps = np.diag([2.0, 2.0])      # GPS noise
        self.R_imu = np.array([[0.01]])        # IMU heading noise

    def predict(self, v: float, omega: float, dt: float):
        """
        Predict using motion model.
        v: linear velocity (from wheel encoders)
        omega: angular velocity (from gyroscope)
        """
        theta = self.x[2, 0]

        # Nonlinear motion model
        # x_new = x + v * cos(theta) * dt
        # y_new = y + v * sin(theta) * dt
        # theta_new = theta + omega * dt
        dx = np.array([
            [v * np.cos(theta) * dt],
            [v * np.sin(theta) * dt],
            [omega * dt]
        ])
        self.x = self.x + dx

        # Normalize theta to [-pi, pi]
        self.x[2, 0] = (self.x[2, 0] + np.pi) % (2 * np.pi) - np.pi

        # Jacobian of motion model (F = df/dx)
        F = np.array([
            [1, 0, -v * np.sin(theta) * dt],
            [0, 1,  v * np.cos(theta) * dt],
            [0, 0, 1]
        ])

        self.P = F @ self.P @ F.T + self.Q
        return self.x.copy()

    def update_gps(self, gps_x: float, gps_y: float):
        """Update with GPS measurement (x, y)."""
        z = np.array([[gps_x], [gps_y]])
        H = np.array([
            [1, 0, 0],
            [0, 1, 0]
        ])
        self._update(z, H, self.R_gps)

    def update_imu_heading(self, heading: float):
        """Update with IMU compass heading."""
        z = np.array([[heading]])
        H = np.array([[0, 0, 1]])

        # Handle angle wrapping in innovation
        y = z - H @ self.x
        y[0, 0] = (y[0, 0] + np.pi) % (2 * np.pi) - np.pi

        S = H @ self.P @ H.T + self.R_imu
        K = self.P @ H.T @ np.linalg.inv(S)
        self.x = self.x + K @ y
        self.x[2, 0] = (self.x[2, 0] + np.pi) % (2 * np.pi) - np.pi
        self.P = (np.eye(3) - K @ H) @ self.P

    def _update(self, z, H, R):
        """Generic update step."""
        y = z - H @ self.x                    # Innovation
        S = H @ self.P @ H.T + R              # Innovation covariance
        K = self.P @ H.T @ np.linalg.inv(S)   # Kalman Gain

        self.x = self.x + K @ y
        self.P = (np.eye(3) - K @ H) @ self.P

    def get_state(self):
        return self.x.flatten()  # [x, y, theta]

Simulation: EKF Sensor Fusion (Encoder + IMU + GPS)

# Mô phỏng robot chạy vòng tròn
np.random.seed(42)
dt = 0.05      # 20 Hz control
duration = 60  # 60 seconds

# Robot parameters
v_true = 0.5           # 0.5 m/s
omega_true = 0.1       # Turning rate (rad/s) → vòng tròn

times = np.arange(0, duration, dt)
n_steps = len(times)

# Ground truth trajectory (circle)
true_x, true_y, true_theta = [0.0], [0.0], [0.0]
for i in range(1, n_steps):
    true_theta.append(true_theta[-1] + omega_true * dt)
    true_x.append(true_x[-1] + v_true * np.cos(true_theta[-1]) * dt)
    true_y.append(true_y[-1] + v_true * np.sin(true_theta[-1]) * dt)

true_x, true_y, true_theta = np.array(true_x), np.array(true_y), np.array(true_theta)

# Noisy sensor readings
encoder_v = v_true + np.random.normal(0, 0.05, n_steps)      # Encoder velocity
imu_omega = omega_true + np.random.normal(0, 0.02, n_steps)  # Gyroscope
gps_x = true_x + np.random.normal(0, 1.5, n_steps)           # GPS x
gps_y = true_y + np.random.normal(0, 1.5, n_steps)           # GPS y
imu_heading = true_theta + np.random.normal(0, 0.1, n_steps) # Compass

# Run EKF
ekf = ExtendedKalmanFilter()
est_x, est_y, est_theta = [], [], []

for i in range(n_steps):
    # Predict with encoder + gyro (every step, 20 Hz)
    ekf.predict(encoder_v[i], imu_omega[i], dt)

    # Update with GPS (every 1 second = every 20 steps)
    if i % 20 == 0:
        ekf.update_gps(gps_x[i], gps_y[i])

    # Update with IMU heading (every 5 steps = 4 Hz)
    if i % 5 == 0:
        ekf.update_imu_heading(imu_heading[i])

    state = ekf.get_state()
    est_x.append(state[0])
    est_y.append(state[1])
    est_theta.append(state[2])

# Plot
fig, axes = plt.subplots(1, 2, figsize=(14, 6))

# Trajectory
axes[0].plot(true_x, true_y, 'g-', linewidth=2, label='Ground Truth')
axes[0].scatter(gps_x[::20], gps_y[::20], c='r', s=15, alpha=0.5, label='GPS')
axes[0].plot(est_x, est_y, 'b-', linewidth=1.5, label='EKF Estimate')
axes[0].set_xlabel('X (m)')
axes[0].set_ylabel('Y (m)')
axes[0].set_title('Robot Trajectory -- EKF Sensor Fusion')
axes[0].legend()
axes[0].set_aspect('equal')
axes[0].grid(True, alpha=0.3)

# Error over time
error = np.sqrt((np.array(est_x) - true_x)**2 + (np.array(est_y) - true_y)**2)
axes[1].plot(times, error, 'b-', label='Position Error (EKF)')
axes[1].set_xlabel('Time (s)')
axes[1].set_ylabel('Error (m)')
axes[1].set_title('Localization Error Over Time')
axes[1].legend()
axes[1].grid(True, alpha=0.3)

plt.tight_layout()
plt.savefig('ekf_sensor_fusion.png', dpi=150)
plt.show()

ROS 2 robot_localization Package

Trong thực tế, bạn không cần implement EKF từ đầu. ROS 2 có package robot_localization cung cấp EKF và UKF nodes sẵn.

Config robot_localization

# ekf.yaml -- Config cho robot_localization EKF node
ekf_filter_node:
  ros__parameters:
    frequency: 30.0
    sensor_timeout: 0.1
    two_d_mode: true   # Robot 2D (ground robot)

    # World frame
    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom

    # Sensor 1: Wheel odometry
    odom0: /wheel_odom
    odom0_config: [true, true, false,   # x, y, z
                   false, false, true,   # roll, pitch, yaw
                   true, true, false,    # vx, vy, vz
                   false, false, true,   # vroll, vpitch, vyaw
                   false, false, false]  # ax, ay, az

    # Sensor 2: IMU
    imu0: /imu/data
    imu0_config: [false, false, false,   # x, y, z
                  false, false, true,    # roll, pitch, yaw
                  false, false, false,   # vx, vy, vz
                  false, false, true,    # vroll, vpitch, vyaw
                  true, true, false]     # ax, ay, az
    imu0_differential: true   # Use differential mode for heading

    # Sensor 3: GPS (optional, requires navsat_transform)
    # odom1: /gps/odom
    # odom1_config: [true, true, false, ...]

    # Process noise
    process_noise_covariance: [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                                0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                                0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                                0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                                0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                                0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0,
                                0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0,
                                0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0,
                                0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0,
                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0,
                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0,
                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0,
                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0,
                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0,
                                0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015]

Launch file

# ekf_launch.py
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    config = os.path.join(
        get_package_share_directory('my_robot_localization'),
        'config', 'ekf.yaml'
    )

    return LaunchDescription([
        Node(
            package='robot_localization',
            executable='ekf_node',
            name='ekf_filter_node',
            output='screen',
            parameters=[config],
            remappings=[
                ('odometry/filtered', '/odom/filtered'),
            ]
        ),
    ])

So sánh KF, EKF, UKF

Trong thực tế, EKF là lựa chọn phổ biến nhất cho robot localization vì cân bằng tốt giữa accuracy và computational cost. UKF chỉ cần khi nonlinearity rất mạnh (ví dụ: underwater robot với complex dynamics).

Tài liệu tham khảo

• Thrun, S., Burgard, W., & Fox, D. (2005). Probabilistic Robotics. MIT Press -- sách kinh điển về robot localization
• arXiv:2410.10409 -- SMART-TRACK: Kalman Filter-Guided Sensor Fusion cho UAV Object Tracking
• arXiv:2509.22693 -- Mobile Robot Localization via Indoor Positioning System and Odometry Fusion

Bài viết liên quan

• PID Control cho Robot: Từ lý thuyết đến thực hành -- Nền tảng điều khiển trước khi sensor fusion
• SLAM Navigation cho Robot -- Bước tiếp theo sau localization
• Nav2 Series: SLAM cho Robot di động -- SLAM chi tiết trong ROS 2
• LiDAR 3D Mapping cho Robot -- Sensor LiDAR và mapping
• Python cho Robot: Từ cơ bản đến ROS 2 -- Lập trình robot với Python