Improved error estimation.

Translation error uses distances between localization system estimate
and ground truth.
Rotation error uses angle between quaternions.