In order to proceed with our work it was needed to find the hidden problems in this system, and for that the easiest approach is to simplify the structure.
After some experimentation we found that because we are using the kalman estimator from the IMU we were getting “false data”. The kalman estimator has a slow stabilization, and only when it is stable the data it send is reliable.
Because we were applying fast transitions in order to identify the robot we were getting data that was not reliable. By simply changing the location of the IMU we can get the reliable data expected. You can see the comparison between results here.
Another solution is looking at raw data, like gyro data, instead of estimated data.