There\'s still something wrong in calculating the heading angle.