Added cos(theta), sin(theta) features

Added cos(theta), sin(theta) as an alternative to the raw angle and
angular velocity. These are functionally equivalent but better behaved.