sensor-fusion
You are sensor-fusion - a specialized skill for multi-sensor fusion and state estimation using Kalman filtering and factor graph optimization.
Overview
This skill enables AI-powered sensor fusion including:
- Implementing Extended Kalman Filter (EKF) for state estimation
- Configuring Unscented Kalman Filter (UKF) for nonlinear systems
- Setting up robot_localization package configuration
- Implementing IMU preintegration and bias estimation
- Configuring GPS/RTK integration with local coordinate frames
- Implementing wheel odometry fusion with slip compensation
- Setting up visual odometry integration
- Configuring outlier rejection (Mahalanobis, chi-squared)
- Tuning process and measurement noise covariances
- Implementing sensor delay compensation
Prerequisites
- ROS2 with robot_localization package
- Calibrated sensors (IMU, cameras, wheel encoders)
- Understanding of coordinate frames (REP-105)
- Sensor noise characteristics
Capabilities
1. robot_localization Configuration
Configure the ROS2 robot_localization package for EKF/UKF:
# ekf_localization.yaml
ekf_filter_node:
ros__parameters:
# Coordinate frames
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
# Frequencies
frequency: 50.0
sensor_timeout: 0.1
two_d_mode: false
# Transform settings
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
publish_tf: true
publish_acceleration: false
# IMU input
imu0: /imu/data
imu0_config: [false, false, false, # x, y, z position
true, true, true, # roll, pitch, yaw
false, false, false, # vx, vy, vz
true, true, true, # vroll, vpitch, vyaw
true, true, true] # ax, ay, az
imu0_differential: false
imu0_relative: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true
# Wheel odometry input
odom0: /wheel_odom
odom0_config: [true, true, false, # x, y, z position
false, false, true, # roll, pitch, yaw
true, true, false, # vx, vy, vz
false, false, true, # vroll, vpitch, vyaw
false, false, false] # ax, ay, az
odom0_differential: false
odom0_relative: false
odom0_queue_size: 10
# GPS input (for map frame)
# odom1: /gps/odom
# odom1_config: [true, true, true,
# false, false, false,
# false, false, false,
# false, false, false,
# false, false, false]
# odom1_differential: false
# Process noise covariance (Q matrix diagonal)
process_noise_covariance: [
0.05, # x
0.05, # y
0.06, # z
0.03, # roll
0.03, # pitch
0.06, # yaw
0.025, # vx
0.025, # vy
0.04, # vz
0.01, # vroll
0.01, # vpitch
0.02, # vyaw
0.01, # ax
0.01, # ay
0.015 # az
]
# Initial estimate covariance (P0 matrix diagonal)
initial_estimate_covariance: [
1e-9, 1e-9, 1e-9, # position
1e-9, 1e-9, 1e-9, # orientation
1e-9, 1e-9, 1e-9, # velocity
1e-9, 1e-9, 1e-9, # angular velocity
1e-9, 1e-9, 1e-9 # acceleration
]2. Two-EKF Setup (Odom + Map Frames)
Configure two EKF instances for continuous odometry and global localization:
# dual_ekf_navsat.yaml
# EKF for continuous odometry (odom frame)
ekf_filter_node_odom:
ros__parameters:
frequency: 50.0
two_d_mode: false
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: odom
# Fuse IMU and wheel odometry only
imu0: /imu/data
imu0_config: [false, false, false,
true, true, true,
false, false, false,
true, true, true,
true, true, true]
imu0_remove_gravitational_acceleration: true
odom0: /wheel_odom
odom0_config: [true, true, false,
false, false, true,
true, true, false,
false, false, true,
false, false, false]
# EKF for global localization (map frame)
ekf_filter_node_map:
ros__parameters:
frequency: 50.0
two_d_mode: false
map_frame: map
odom_frame: odom
base_link_frame: base_link
world_frame: map
# Fuse odometry output and GPS
odom0: /odometry/filtered
odom0_config: [true, true, true,
true, true, true,
true, true, true,
true, true, true,
true, true, true]
odom1: /gps/odom
odom1_config: [true, true, true,
false, false, false,
false, false, false,
false, false, false,
false, false, false]
odom1_differential: false
# NavSat transform node
navsat_transform_node:
ros__parameters:
frequency: 50.0
delay: 0.0
magnetic_declination_radians: 0.0
yaw_offset: 0.0
zero_altitude: true
broadcast_utm_transform: true
publish_filtered_gps: true
use_odometry_yaw: false
wait_for_datum: false3. Custom EKF Implementation
Implement a custom EKF for state estimation:
import numpy as np
from scipy.linalg import block_diag
class RobotEKF:
"""Extended Kalman Filter for robot localization."""
def __init__(self, dt=0.02):
self.dt = dt
# State: [x, y, z, roll, pitch, yaw, vx, vy, vz, wx, wy, wz]
self.n_states = 12
# State vector
self.x = np.zeros(self.n_states)
# State covariance
self.P = np.eye(self.n_states) * 0.1
# Process noise
self.Q = np.diag([
0.01, 0.01, 0.01, # position noise
0.001, 0.001, 0.001, # orientation noise
0.1, 0.1, 0.1, # velocity noise
0.01, 0.01, 0.01 # angular velocity noise
])
def predict(self, u=None):
"""Predict step using motion model."""
dt = self.dt
x, y, z = self.x[0:3]
roll, pitch, yaw = self.x[3:6]
vx, vy, vz = self.x[6:9]
wx, wy, wz = self.x[9:12]
# State transition (constant velocity model)
# Position update
self.x[0] += vx * np.cos(yaw) * dt - vy * np.sin(yaw) * dt
self.x[1] += vx * np.sin(yaw) * dt + vy * np.cos(yaw) * dt
self.x[2] += vz * dt
# Orientation update
self.x[3] += wx * dt
self.x[4] += wy * dt
self.x[5] += wz * dt
# Jacobian of state transition
F = self._compute_jacobian()
# Covariance prediction
self.P = F @ self.P @ F.T + self.Q
def _compute_jacobian(self):
"""Compute Jacobian of state transition."""
dt = self.dt
yaw = self.x[5]
vx, vy = self.x[6:8]
F = np.eye(self.n_states)
# Position derivatives w.r.t. yaw
F[0, 5] = -vx * np.sin(yaw) * dt - vy * np.cos(yaw) * dt
F[1, 5] = vx * np.cos(yaw) * dt - vy * np.sin(yaw) * dt
# Position derivatives w.r.t. velocity
F[0, 6] = np.cos(yaw) * dt
F[0, 7] = -np.sin(yaw) * dt
F[1, 6] = np.sin(yaw) * dt
F[1, 7] = np.cos(yaw) * dt
F[2, 8] = dt
# Orientation derivatives w.r.t. angular velocity
F[3, 9] = dt
F[4, 10] = dt
F[5, 11] = dt
return F
def update_imu(self, imu_data):
"""Update with IMU measurement (orientation, angular velocity, acceleration)."""
# Measurement: [roll, pitch, yaw, wx, wy, wz, ax, ay, az]
z = np.array([
imu_data['roll'], imu_data['pitch'], imu_data['yaw'],
imu_data['wx'], imu_data['wy'], imu_data['wz']
])
# Measurement matrix
H = np.zeros