ROS Answers: Open Source Q&A Forum - RSS feedhttps://answers.ros.org/questions/Open source question and answer forum written in Python and DjangoenROS Answers is licensed under Creative Commons Attribution 3.0Fri, 24 Feb 2023 07:30:37 -0600Extended Kalman Filter Robot Localization Drifthttps://answers.ros.org/question/412844/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.Fri, 24 Feb 2023 07:30:37 -0600https://answers.ros.org/question/412844/extended-kalman-filter-robot-localization-drift/