🚧 EKFCore — A Templated Extended Kalman Filter Library

EKFCore is a modular and high-performance C++17 implementation of a templated Extended Kalman Filter (EKF) for state estimation. It provides a generic, reusable, and dimension-safe framework for fusing noisy sensor data to obtain accurate and reliable state estimates in uncertain environments — a cornerstone of robotics and autonomous systems.

The library serves as both a learning resource and a practical foundation for implementing sensor fusion pipelines in real-world systems such as robots, drones, and autonomous vehicles.

---

Motivation

Modern autonomous systems rely on accurate estimates of their internal state (position, velocity, orientation, etc.) to navigate safely. However, real-world sensors — GPS, IMU, wheel encoders — are inherently noisy and often inconsistent. The challenge is to combine multiple imperfect measurements into a consistent and smooth estimate of the true system state.

---

The EKF Solution

The Extended Kalman Filter (EKF) provides an elegant and powerful solution by operating in two alternating steps:

1. Prediction: Use the motion model to predict the system’s next state.
2. Update: Correct that prediction using sensor measurements.

By linearizing nonlinear system dynamics through Jacobian matrices, the EKF maintains both computational efficiency and robustness, making it one of the most widely used estimators in robotics, aerospace, and control systems.

---

Key Features

• 🔢 Templated C++ EKF Implementation A from-scratch EKF built using templates and the Eigen library, ensuring compile-time dimensional safety, flexibility, and high performance.

• 🧮 Correlated Noise Generation Includes generateCorrelatedNoise, which uses Cholesky decomposition to generate statistically valid noise vectors based on user-defined covariance matrices.

• 🧠 Robust Sensor Fusion Framework Designed to fuse data from multiple heterogeneous sensors (e.g., GPS, IMU, wheel odometry) with independent measurement models.

• 🧰 Simulation Environment A configurable simulator for ground truth generation and realistic noise injection to model real-world disturbances.

• 📊 Comprehensive Data Output Outputs ground truth, noisy sensor data, and EKF estimates (with covariance matrices) to CSV files for post-processing and analysis.

• 📈 Visualization Tools (Python) Python scripts using matplotlib, numpy, and pandas visualize:

  • Ground truth trajectory
  • Noisy sensor data
  • Filtered EKF estimates
  • Covariance ellipses representing uncertainty

---

🧮 Mathematical Formulation

State Vector (2D Vehicle Example)

For a planar robot:

$$ X = \begin{bmatrix} x y \theta \end{bmatrix} $$

where $(x, y)$ represent position and $\theta$ represents orientation.

---

Prediction (Motion Model)

The motion model evolves the state according to control inputs (velocity $v$, angular velocity $\omega$):

$$ X_{k+1} = f(X_k, u_k) + w_k $$

where $w_k \sim \mathcal{N}(0, Q_k)$ is process noise. Linearization gives the Jacobian used in the prediction step:

$$ F_t = \frac{\partial f}{\partial X} $$

---

Update (Measurement Models)

Multiple sensor models are supported, such as:

• GPS / IMU:

  $$ Z_{gps} = h_{gps}(X_k) + v_{gps} = X_k + v_{gps} $$

• Wheel Odometry:

  $$ Z_{odom} = h_{odom}(X_k) + v_{odom} $$

  where $(v_{odom}, v_{gps}) \sim \mathcal{N}(0, R_k)$ represent measurement noise.

Each sensor has its own measurement Jacobian:

$$ H_t = \frac{\partial h}{\partial X} $$

This Jacobian linearizes the measurement model around the current state estimate and is used during the update step to correct the predicted state.

---

🚀 Getting Started

Prerequisites

Ensure you have the following installed:

• C++17 or newer

• CMake ≥ 3.10

• Eigen 3 (automatically fetched by CMake or installed manually)

• Python 3.x with:

  pip install matplotlib numpy pandas

---

Building the Project

# Clone repository
git clone https://github.com/0xphen/EKFCore.git
cd EKFCore

# Configure build
mkdir build && cd build
cmake ..

# Build executable
make

---

Run the Simulation

After building, an executable ekfcore_sim will be generated in the build directory:

./ekfcore_sim

This will produce:

• Ground truth and noisy sensor data
• Filtered EKF estimates
• Covariance matrices

All results are exported as CSV files for visualization.

---

Visualize Results

Run the provided Python script to visualize results:

python3 scripts/plot_results.py

This will plot:

• Ground Truth Path
• Noisy Sensor Data
• EKF Estimated Path
• Covariance Ellipses

---

Future Work

• Integrate IMU bias estimation
• Add Unscented Kalman Filter (UKF) variant
• Provide ROS2 integration example
• Add unit tests for measurement model consistency

---

📚 References

• Probabilistic Robotics — Thrun, Burgard & Fox
• State Estimation for Robotics — Tim Barfoot
• Estimation and Control of Dynamic Systems — Goodwin & Sin
• Kalman Filter Tutorial — Simon D. (IEEE)

---