implemented stabilization functionality. Still there is some drift when using it.