Extended Kalman Filter for Accelerometer and Gyro data