ROS2-EKF-Fusion

• 

  gazebo & rviz image

Inspiration

As a 3rd-year Computer Engineering student and a finalist in the TEKNOFEST Robotaxi Autonomous Vehicle Competition, I realized that writing code that "works" is not enough for autonomous systems; we need code that is "safe" and "fast."

While my experience with Python in ROS was strong, I noticed that real-time systems require the performance of C++. I wanted to step out of my comfort zone and master the industry-standard C++ language. My goal was to leverage the power of memory management and strict typing to solve a classic robotics problem: Sensor Noise.

What it does

ROS2-EKF-Fusion is a real-time state estimation node. In the simulation environment:

1. It listens to noisy data from the Wheel Odometry (encoders) and IMU (accelerometer/gyroscope) of a TurtleBot 3 robot.
2. It fuses these two data sources using an Extended Kalman Filter (EKF) algorithm.
3. It publishes a precise, filtered trajectory (Green Line) that eliminates drift and sensor errors, providing a much smoother path than raw data (Red Line).

How I built it

I built the project on the ROS 2 Humble framework using C++17.

• The Math: I implemented the prediction and update steps of the Kalman Filter using the Eigen library for high-performance matrix operations. The state vector (x) consists of position and velocity:

$$ x = [p_x, p_y, v_x, v_y]^T $$

• The Logic:

  • Prediction: Based on the motion model:

  $$ \hat{x}{k|k-1} = F_k \hat{x}{k-1|k-1} $$

  • Update: Correcting the estimate with sensor data using the Kalman Gain (K):

  $$ K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R_k)^{-1} $$

• The Tools: I used Gazebo for simulation and Rviz2 for real-time visualization of the filtered pose.

Challenges I ran into

Transitioning from Python to C++ was the biggest challenge:

• Matrix Dimensions: Unlike Python's NumPy, the Eigen library in C++ is strict about dimensions at compile-time. Debugging template errors taught me a lot about type safety.
• Memory Management: Understanding std::shared_ptr and how to pass data efficiently between ROS nodes without memory leaks was a steep learning curve.
• Tuning Matrices: Adjusting the Covariance Matrices ((Q) and (R)) to find the right balance between trusting the "Model" vs. trusting the "Sensor" took significant trial and error.

Accomplishments that I'm proud of

• Successfully porting a complex robotics algorithm from Python to C++.
• Implementing a custom math backend using Eigen instead of using a black-box package.
• Seeing the Green Line (Filtered Path) in Rviz stay true to the robot's path while the raw odometry drifted away.

What I learned

This project was a crash course in:

• Low-Level Robotics: How nav_msgs and sensor_msgs are structured in memory.
• Linear Algebra: Applying theoretical matrix inversion and multiplication to a real-world navigation problem.
• ROS 2 Architecture: The importance of Node composition and lifecycle in C++.

What's next for ROS2-EKF-Fusion

• Testing the code on a physical TurtleBot 4.
• Integrating a Visual Odometry source (Camera) to improve accuracy further.
• Optimizing the code for embedded systems like Jetson Nano.

Built With

• algorithms
• c++
• cmake
• eigen
• gazebo
• git
• github
• kalman-filter
• robotic
• ros2
• rviz2
• sensor-fusion
• turtlebot3
• ubuntu