# Extended Kalman Filter Robot Localization Drift

I have implemented an EKF for robot localization in the style of `robot_localization`

using the famous C++ template`kalman`

library.

My state vector is 15 dimensional, including position, euler angles orientation, velocity (in robot frame), euler angle velocities (in robot frame), accelerations (robot frame)

I am using Gazebo as virtual environment to simulate IMU accelerometer + gyroscope readings, wheel encoders velocities and fake GPS readings by adding noise to base_link states taken from gazebo.

Jacobians have been derived using `sympy`

symbolic toolbox and correctly implemented for `kalman`

library.

Process Noise and Measurement Covariances have been setup to be with a non zero, reasonably small diagonal.

However, my robot state estimate **drifts away very rapidly.**

My guess is that the problem relies on the acceleration measurement model function. Mine look like this:

h_acc(x) = a + R(q)^T*g

where

- x is my state vector
- a the robot accelerations
- R(q) the rotation matrix from body to world frame using estimated orientation q:[roll, pitch, yaw]
- g is the gravity acceleration in ENU : [0 0 +9.8]

My robot starts in a slightly non-flat position, and part of gravity acceleration is sensed in the x-axis (assuming zero acc bias). However, my initial estimated orientation is flat q = [0, 0, 0], so my accelerometer measurement model will fit part of the gravity to an acceleration along x as long as the estimated orientation is not correct.

The estimated robot pose starts pitching up and drifting along x, exploding.

How can I solve this issue? I tried adding to the filter a constant acc bias but didn't help.

Thanks everyone.