Finally i made a angle estimation module to check the angle error and response time. This makes it much easier to make the final adjustments.