fix(base): angle_std returned nan for identical angles (#254)

## The problem

`angle_std` of a set of identical angles can return `nan`:

```python
>>> import numpy as np
>>> from spatialmath.base import angle_std
>>> angle_std(np.full(100, -2.9))
nan          # RuntimeWarning: invalid value encountered in sqrt
```

`R`, the length of the mean of the unit vectors, is at most 1, but
rounding can make it `1 + 2e-16`. Then `-2 * log(R)` is a tiny negative
number and its square root is `nan`. The answer should be 0.

I counted 33 failures in 378 combinations of angle (`-3.1` to `3.1`) and
sample size (1 to 100).

## The fix

Clamp the value under the square root at zero. `R == 0` still gives
`inf`, as before.

## Test

`test_angle_std_identical_angles` runs the 378 combinations and fails on
the old code and passes now. The existing `test_angle_stats` still
passes (24 tests in the file).

Found while reviewing a particle filter change in the Robotics Toolbox
that uses `angle_std`.

Author: petercorke

spatialmath/base/vectors.py

@@ -698,7 +698,10 @@ def angle_std(theta: ArrayLike) -> float:
     Y = np.sin(theta).mean()
     R = np.sqrt(X**2 + Y**2)
 
-    return np.sqrt(-2 * np.log(R))
+    # R is the length of an average of unit vectors so is at most 1, but
+    # rounding can make it 1 + eps, which would give the square root of a
+    # (tiny) negative number and nan
+    return np.sqrt(max(0.0, -2 * np.log(R)))
 
 
 def angle_mean(theta: ArrayLike) -> float:

tests/base/test_vectors.py

@@ -7,6 +7,7 @@
 
 """
 
+import warnings
 import numpy as np
 import numpy.testing as nt
 import unittest
@@ -397,6 +398,17 @@ def test_angle_stats(self):
         self.assertAlmostEqual(angle_mean(theta), pi)
         self.assertAlmostEqual(angle_std(theta), 0.9717284050981313)
 
+    def test_angle_std_identical_angles(self):
+        # the length of the mean unit vector can come out as 1 + eps, which
+        # used to give the square root of a negative number
+        for angle in np.linspace(-3.1, 3.1, 63):
+            for n in (1, 2, 3, 7, 10, 100):
+                with warnings.catch_warnings():
+                    warnings.simplefilter("error")
+                    std = angle_std(np.full(n, angle))
+                self.assertTrue(np.isfinite(std))
+                self.assertLess(std, 1e-6)
+
     def test_removesmall(self):
         v = np.r_[1, 2, 3]
         nt.assert_array_almost_equal(removesmall(v), v)