I have implemented an EKF for robot localization in the style of robot_localization using the famous C++ template kalman library.
My state vector $x$ 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 $R$ and measurement covariances $R_m$ have been setup to be with a non zero, small, dynamic_reconfigure-able diagonal values.
Also the initial state $x_0$ is null with very small diagonal covariance $P_0$.
However, my robot state estimate drifts away very rapidly. The estimated robot pose starts pitching up and drifting along x, becoming unstable.
My guess is that the problem relies on the acceleration measurement model function. Mine look like this, assuming zero bias:
$$ h(x) = a + R(q)^\top 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 : $\begin{bmatrix} 0 & 0 & +9.8 \end{bmatrix}^\top$
My robot starts in a slightly non-flat position, thus part of gravity acceleration is sensed in the x-axis.
However, my initial estimated orientation is flat $q = [0, 0, 0]$, so my accelerometer measurement model will sense part of the gravity as an acceleration along x as long as the estimated orientation is not correct.
How can I solve this issue? I tried adding to the filter a constant accelerometer bias but didn't help.
Should I just increase initial $P_0$ for orientation?
Thanks everyone.
