PID gains of Steering and linear velocity must be set better than before. Looks like working but should be examined more detailed.