Kalman filter latency compensation for model matrix (position + orientation)