added drift compensation of Tz and bias compensation of other axes