Fixed some bugs in attitude measurement