Fixed bugs in attitude estimator hw, working well with segway.