Removed RotationVectorDiff
Fixed unit tests
Merge pull request #8 from ethz-asl/develop

Develop