Tim

Results 2 issues of Tim

When rotating in-place and going slightly backwards AMCL estimates a forward motion (albeit a small one). This eventually leads to a completely wrong estimate. Proposed fix is in #745

amcl

This allows to write more concise testing code. Quaternions are compared based on their coefficients.