New function angDistance(); and unit-tests
CRobot2DPoseEstimator: Fix potential bug in w estimation