sensor-fusion

name	sensor-fusion
description	Expert skill for multi-sensor fusion and state estimation using Kalman filtering. Implement EKF/UKF, configure robot_localization, fuse IMU, GPS, odometry, and visual sensors for robust localization.
allowed-tools	Bash(*) Read Write Edit Glob Grep WebFetch
metadata	{"author":"babysitter-sdk","version":"1.0.0","category":"state-estimation","backlog-id":"SK-009"}

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
                  ,  ,  ]   
     
     
     
     

    
     
     [,  ,  ,  
                   , , ,   
                   ,  ,  ,  
                   , , ,   
                   , , ]  
     
     
     

    
    
    
    
    
    
    
    

    
     [
      ,   
      ,   
      ,   
      ,   
      ,   
      ,   
      ,  
      ,  
      ,   
      ,   
      ,   
      ,   
      ,   
      ,   
         
    ]

    
     [
      , , ,     
      , , ,     
      , , ,     
      , , ,     
      , ,       
    ]

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,
                   , , ]



  
     
     

     
     
     
     

    
     
     [,  ,  ,
                   ,  ,  ,
                   ,  ,  ,
                   ,  ,  ,
                   ,  ,  ]

     
     [,  ,  ,
                   , , ,
                   , , ,
                   , , ,
                   , , ]
     



  
     
     
     
     
     
     
     
     
     

3. 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[:]
        roll, pitch, yaw = .x[:]
        vx, vy, vz = .x[:]
        wx, wy, wz = .x[:]

        
        
        .x[] += vx * np.cos(yaw) * dt - vy * np.sin(yaw) * dt
        .x[] += vx * np.sin(yaw) * dt + vy * np.cos(yaw) * dt
        .x[] += vz * dt

        
        .x[] += wx * dt
        .x[] += wy * dt
        .x[] += wz * dt

        
        F = ._compute_jacobian()

        
        .P = F @ .P @ F.T + .Q

     ():
        
        dt = .dt
        yaw = .x[]
        vx, vy = .x[:]

        F = np.eye(.n_states)

        
        F[, ] = -vx * np.sin(yaw) * dt - vy * np.cos(yaw) * dt
        F[, ] = vx * np.cos(yaw) * dt - vy * np.sin(yaw) * dt

        
        F[, ] = np.cos(yaw) * dt
        F[, ] = -np.sin(yaw) * dt
        F[, ] = np.sin(yaw) * dt
        F[, ] = np.cos(yaw) * dt
        F[, ] = dt

        
        F[, ] = dt
        F[, ] = dt
        F[, ] = dt

         F

     ():
        
        
        z = np.array([
            imu_data[], imu_data[], imu_data[],
            imu_data[], imu_data[], imu_data[]
        ])

        
        H = np.zeros((, .n_states))
        H[:, :] = np.eye()  
        H[:, :] = np.eye()  

        
        R = np.diag([, , , , , ])

        ._ekf_update(z, H, R)

     ():
        
        z = np.array([odom_data[], odom_data[], odom_data[]])

        
        H = np.zeros((, .n_states))
        H[, ] =   
        H[, ] =   
        H[, ] =   

        
        R = np.diag([, , ])

        ._ekf_update(z, H, R)

     ():
        
        z = np.array([gps_data[], gps_data[], gps_data[]])

        
        H = np.zeros((, .n_states))
        H[:, :] = np.eye()

        
        R = np.diag([, , ])

        
        y = z - H @ .x
        S = H @ .P @ H.T + R
        mahal_dist = np.sqrt(y.T @ np.linalg.inv(S) @ y)

         mahal_dist < :  
            ._ekf_update(z, H, R)
        :
            ()

     ():
        
        
        y = z - H @ .x

        
        S = H @ .P @ H.T + R

        
        K = .P @ H.T @ np.linalg.inv(S)

        
        .x = .x + K @ y

        
        I_KH = np.eye(.n_states) - K @ H
        .P = I_KH @ .P @ I_KH.T + K @ R @ K.T

     ():
        
         {
            : .x[:],
            : .x[:],
            : .x[:],
            : .x[:],
            : np.diag(.P)
        }

4. IMU Preintegration

Implement IMU preintegration for efficient optimization:

import numpy as np
from scipy.spatial.transform import Rotation

class IMUPreintegration:
    """IMU preintegration for factor graph optimization."""

    def __init__(self, acc_noise=0.01, gyro_noise=0.001,
                 acc_bias_noise=0.0001, gyro_bias_noise=0.00001):
        self.acc_noise = acc_noise
        self.gyro_noise = gyro_noise
        self.acc_bias_noise = acc_bias_noise
        self.gyro_bias_noise = gyro_bias_noise

        self.reset()

    def reset(self):
        """Reset preintegration."""
        self.delta_R = np.eye(3)
        self.delta_v = np.zeros(3)
        self.delta_p = np.zeros(3)
        self.delta_t = 0.0

        # Jacobians for bias correction
        self.dR_dbg = np.zeros((3, 3))
        self.dv_dba = np.zeros((3, 3))
        self.dv_dbg = np.zeros((3, 3))
        self.dp_dba = np.zeros((3, 3))
        self.dp_dbg = np.zeros((, ))

        
        .cov = np.zeros((, ))

     ():
        
         acc_bias  :
            acc_bias = np.zeros()
         gyro_bias  :
            gyro_bias = np.zeros()

        
        acc_unbiased = acc - acc_bias
        gyro_unbiased = gyro - gyro_bias

        
        theta = gyro_unbiased * dt
        dR = Rotation.from_rotvec(theta).as_matrix()

        
        .delta_p += .delta_v * dt +  * .delta_R @ acc_unbiased * dt**
        .delta_v += .delta_R @ acc_unbiased * dt
        .delta_R = .delta_R @ dR
        .delta_t += dt

        
        ._update_jacobians(acc_unbiased, gyro_unbiased, dt)

        
        ._update_covariance(dt)

     ():
        
        
         ():
             np.array([
                [, -v[], v[]],
                [v[], , -v[]],
                [-v[], v[], ]
            ])

        theta = gyro * dt
        Jr = ._right_jacobian(theta)

        
        .dR_dbg = .delta_R.T @ .dR_dbg - Jr * dt

        
        .dv_dba = .dv_dba - .delta_R * dt
        .dv_dbg = .dv_dbg - .delta_R @ skew(acc) @ .dR_dbg * dt

        
        .dp_dba = .dp_dba + .dv_dba * dt -  * .delta_R * dt**
        .dp_dbg = .dp_dbg + .dv_dbg * dt -  * .delta_R @ skew(acc) @ .dR_dbg * dt**

     ():
        
        angle = np.linalg.norm(theta)
         angle < :
             np.eye()

        axis = theta / angle
        s = np.sin(angle)
        c = np.cos(angle)

         (s / angle) * np.eye() + \
               ( - s / angle) * np.outer(axis, axis) + \
               (( - c) / angle) * ._skew(axis)

     ():
        
         np.array([
            [, -v[], v[]],
            [v[], , -v[]],
            [-v[], v[], ]
        ])

     ():
        
        
        A = np.eye()
        B = np.eye() * dt

        noise_cov = np.diag([
            .gyro_noise**, .gyro_noise**, .gyro_noise**,
            .acc_noise**, .acc_noise**, .acc_noise**,
            .gyro_bias_noise**, .gyro_bias_noise**, .gyro_bias_noise**
        ])

        .cov = A @ .cov @ A.T + B @ noise_cov @ B.T

     ():
        
         {
            : .delta_R,
            : .delta_v,
            : .delta_p,
            : .delta_t,
            : .cov,
            : {
                : .dR_dbg,
                : .dv_dba,
                : .dv_dbg,
                : .dp_dba,
                : .dp_dbg
            }
        }

5. Noise Covariance Tuning

Guidelines for tuning process and measurement noise:

def tune_noise_covariances(sensor_data_log, initial_Q, initial_R):
    """
    Autotuning for noise covariances using innovation analysis.

    Parameters:
    - sensor_data_log: List of sensor measurements
    - initial_Q: Initial process noise covariance
    - initial_R: Initial measurement noise covariance

    Returns:
    - Tuned Q and R matrices
    """
    from scipy.optimize import minimize

    def compute_nees(Q_diag, R_diag, data):
        """Compute Normalized Estimation Error Squared."""
        ekf = RobotEKF()
        ekf.Q = np.diag(Q_diag)

        nees_values = []
        for measurement in data:
            ekf.predict()

            # Compute innovation
            z = measurement['z']
            H = measurement['H']
            R = np.diag(R_diag[:len(z)])

            y = z - H @ ekf.x
            S = H @ ekf.P @ H.T + R

            # NEES
            nees = y.T @ np.linalg.inv(S) @ y / len(z)
            nees_values.append(nees)

            ekf._ekf_update(z, H, R)

        return np.mean(nees_values)

    def objective(params):
        n_Q = len(initial_Q)
        Q_diag = params[:n_Q]
        R_diag = params[n_Q:]

        nees = compute_nees(Q_diag, R_diag, sensor_data_log)
        # NEES should be close to 1 for consistent estimation
        return (nees - 1.0)**2

    initial_params = np.concatenate([np.diag(initial_Q), np.diag(initial_R)])

    result = minimize(objective, initial_params,
                     method='L-BFGS-B',
                     bounds=[(1e-6, 10)] * len(initial_params))

    n_Q = len(initial_Q)
    Q_tuned = np.diag(result.x[:n_Q])
    R_tuned = np.diag(result.x[n_Q:])

     Q_tuned, R_tuned

6. Launch Configuration

Launch robot_localization with sensor fusion:

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():
    pkg_share = FindPackageShare('my_robot_localization')

    ekf_config = PathJoinSubstitution([pkg_share, 'config', 'ekf.yaml'])

    return LaunchDescription([
        # EKF for odometry frame
        Node(
            package='robot_localization',
            executable='ekf_node',
            name='ekf_filter_node_odom',
            output='screen',
            parameters=[ekf_config],
            remappings=[
                ('odometry/filtered', 'odometry/local'),
                ('accel/filtered', 'accel/local')
            ]
        ),

        # EKF for map frame (with GPS)
        Node(
            package='robot_localization',
            executable='ekf_node',
            name='ekf_filter_node_map',
            output='screen',
            parameters=[ekf_config],
            remappings=[
                ('odometry/filtered', 'odometry/global'),
                ('accel/filtered', 'accel/global')
            ]
        ),

        # NavSat transform for GPS
        Node(
            package='robot_localization',
            executable='navsat_transform_node',
            name='navsat_transform_node',
            output='screen',
            parameters=[ekf_config],
            remappings=[
                (, ),
                (, ),
                (, )
            ]
        )
    ])

MCP Server Integration

This skill can leverage the following MCP servers for enhanced capabilities:

Server	Description	Reference
ros-mcp-server	ROS topic access	GitHub

Best Practices

1. Sensor calibration - Accurate calibration is essential for fusion quality
2. Noise characterization - Measure actual sensor noise statistics
3. Frame conventions - Follow REP-105 for coordinate frames
4. Outlier rejection - Implement Mahalanobis distance checks
5. Time synchronization - Ensure sensors are time-synchronized
6. Graceful degradation - Handle sensor failures gracefully

Process Integration

This skill integrates with the following processes:

• sensor-fusion-framework.js - Primary fusion framework
• visual-slam-implementation.js - VIO fusion
• lidar-mapping-localization.js - LiDAR-inertial fusion
• robot-calibration.js - Sensor calibration

Output Format

When executing operations, provide structured output:

{
  "operation": "configure-fusion",
  "filterType": "EKF",
  "status": "success",
  "sensors": {
    "imu": {"fused": true, "rate": 200},
    "odom": {"fused": true, "rate": 50},
    "gps": {"fused": true, "rate": 5}
  },
  "artifacts": [
    "config/ekf_localization.yaml"
    
  
   
    
    
  

Constraints

• Verify sensor time synchronization before fusion
• Ensure coordinate frame consistency (REP-105)
• Monitor filter divergence indicators
• Test outlier rejection with ground truth
• Validate covariance growth during sensor dropout