Added wrap to [-pi,pi) for estimated theta. Fixes #29