Merge changes Id5584bc1,I8ca826d0

* changes:
  revert parts of dc5b63e40, which made gyro drift estimation unstable
  use gyro timestamp directly in fusion