Added wrapToPi for theta output to match real system behavior.