IMU integration and Kalman Covariance setup