sensor-fusion

What it does

The skill provides multi-sensor fusion and state estimation using Kalman filtering and factor graph optimization. It includes implementations for Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF), configuration of the robot_localization package, IMU preintegration with bias estimation, GPS/RTK integration, wheel odometry fusion with slip compensation, and visual odometry integration. It also covers outlier rejection and covariances tuning, along with sensor delay compensation.

How it works

• robot_localization Configuration: Defines a YAML setup for EKF/UKF in ROS2, wiring IMU (/imu/data) and wheel odometry (/wheel_odom) into an EKF, including transform and noise parameters, plus a dual EKF setup: one for continuous odometry in the odom frame and another for global/localization in the map frame, and a NavSat transform node for GPS handling.
• Two-EKF Setup: Provides two separate EKF configurations (ekf_filter_node_odom and ekf_filter_node_map) to fuse IMU and odometry for odom frame, and to fuse odometry with GPS for global localization, respectively.
• Custom EKF Implementation: A Python-based Extended Kalman Filter class (RobotEKF) handling state as [x, y, z, roll, pitch, yaw, vx, vy, vz, wx, wy, wz], with a constant-velocity motion model, Jacobian calculation, IMU/odometry/GPS update methods, and a robust covariance update using Joseph form.
• IMU Preintegration: An IMUPreintegration class that accumulates delta_R, delta_v, delta_p over time using accelerometer and gyroscope data, with bias handling, Jacobians for bias correction, and a simplified covariance propagation for preintegrated measurements.
• Noise Covariance Tuning: Provides a function scaffold (tune_noise_covariances) for autonomously tuning Q and R via innovation analysis, using NEES/NEES-related metrics within an EKF context.

When to use it

Use when you need robust localization by fusing IMU, wheel odometry, GPS, and visual sensors in a ROS2 environment. Enable two EKFs for continuous odometry and global localization, and leverage IMU preintegration for optimization-based estimation.

What it can touch

• robot_localization package configuration files (YAML blocks in EKF setups)
• Python modules for RobotEKF and IMUPreintegration (predict and update steps with sensor inputs)
• GPS/NavSat transform node parameters for map/world frame alignment
• Data inputs: IMU (/imu/data), wheel odometry (/wheel_odom), odometry (/odometry/filtered), GPS (/gps/odom), and potentially visual odometry inputs (not explicitly spelled as a separate node but included in the overview)

Caveats

• Requires ROS2 with robot_localization and calibrated sensors (IMU, cameras, wheel encoders).
• The GPS update includes an outlier rejection step using Mahalanobis distance for robustness.
• The custom EKF and preintegration code assumes certain state representations and noise models; integration with real robot data may require adjustments to sensor topics and covariances.
• The skill’s content includes sizable code blocks; ensure compatibility with existing codebase styles and dependencies.