Added angle2dcm and dcm2angle functions.
Added omega2rates function.
Added wrapToPi function.

Fix vectorization for quat2angle
Merge branch 'func_dev' of https://github.com/NavPy/NavPy into func_dev