Fixed bug: imu bias should be substracted from measurements