Error-State Kalman Filter (ESKF) with Quaternion-Based Orientation

Everything here has been updated from the previous garbage code that I had for several months and never updated. Bulk of this ReadMe will be a brief summary of setting up the program and a little conceptual understanding of the ESKF. For following me on the thought process of the program archietecture and a deeper conceptual undestanding of the ESKF, it will be at the top and all around of each .cpp file in /Final_Implementation. Currently the .cpp files are being refurbished but if you wanna see the fully functional python implementation you can check /Draft_Implementation. Enjoy the work done so far.

Data

Overview

This ESKF implementation uses quaternion representation for orientation estimation, avoiding gimbal lock issues associated with Euler angles. The filter predicts state using gyroscope and accelerometer data, then corrects the estimates using GPS data. Intuitive explanation is in filter.py comments.

State Vector (16 elements):

• Position (3D): [x, y, z]
• Orientation (quaternion): [qw, qx, qy, qz]
• Velocity (3D): [vx, vy, vz]
• Accelerometer bias: [bax, bay, baz]
• Gyroscope bias: [bwx, bwy, bwz]

Error State (15 elements):

• Position error (3D)
• Orientation error (3D rotation vector)
• Velocity error (3D)
• Accelerometer bias error (3D)
• Gyroscope bias error (3D)

Theoretical Background

This project could not have been done without these resources (The first link is absolutely goated on general KF). The second link is for implementation. Also check out chapter 6 for implementing update step. ThreeBlueOneBrown does an absolutely amazing explanation on quaternions visualzed and somehow gives us 3d people a somewhat grasp of a 4d concept.

1. Basic Kalman Filter Intuition: How a Kalman Filter Works (in Pictures)
2. ESKF with Quaternions (Sections 5.1-5.4.3): Quaternion kinematics for the error-state Kalman filter
3. ESKF Generalized Formulas: IEEE Paper on ESKF
4. Quaternion Intuition: Visualizing Quaternions
5. Jacobian Matrix Intuition: Jacobian Matrix Explained
6. Extended Kalman Filter Visualization: Extended Kalman Filter Visualized

Installation

1. Clone the repository:

git clone https://github.com/hyunlee8/ErrorStateKalmanFilter.git
cd ErrorStateKalmanFilter

2. Install required dependencies:

pip install numpy pandas pyquaternion

### Running the Filter

1. Prepare your sensor data files in the `src/` directory:
   - `IMU_data.csv` - IMU sensor measurements
   - `motion_data.csv` - Position/velocity measurements (optional for updates)

2. Run the filter:
```bash
cd src
python main.py

Parameter Guidelines:

• sig_a_noise: Set based on your accelerometer datasheet noise density
• sig_w_noise: Set based on your gyroscope datasheet noise density
• sig_a_walk: Accelerometer bias instability (how fast bias drifts)
• sig_w_walk: Gyroscope bias instability (how fast bias drifts)
• gravity: Local gravitational acceleration (typically 9.81 m/s²)

**One important caveat for gravity is that typically in most papers you will see that gravity is included in the error states however when computing it almost never affects the kalman gain or any other values when applied. Also it only adds unecessary complexity thus is not inluded in delta X.

Algorithm Details

Prediction Step ~~ explained better in src/filter/filter.py

1. Remove biases from IMU measurements
2. Transform acceleration to global frame using current orientation
3. Integrate position and velocity using kinematic equations
4. Update orientation via quaternion multiplication
5. Propagate covariance using linearized error-state dynamics

Update Step ~~ explained better in src/filter/filter.py

1. Compute innovation (measurement - prediction)
2. Calculate Kalman gain using current covariance and measurement noise
3. Update error state with innovation
4. Inject error state into nominal state:
   • Position: direct addition
   • Orientation: quaternion multiplication
   • Velocity and biases: direct addition
5. Update covariance using Joseph form (numerically stable)
6. Reset error state to zero