Add angle compensation function
