Accelerometer/Gyroscope reading and comp. filter