Extended Kalman Filter Robot Localization Drift
I have implemented an EKF for robot localization in the style of robot_localization using the famous C++ templatekalman 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.
