Support state addition not using + issue #197

I've added state_add as an attribute to the class to support
this use case.

Author: rlabbe

filterpy/kalman/UKF.py

@@ -136,6 +136,12 @@ def residual(a, b):
                     y += 2*np.pi
                 return y
 
+    state_add: callable (x, y), optional, default np.add
+        Function that subtracts two state vectors, returning a new
+        state vector. Used during update to compute `x + K@y`
+        You will have to supply this if your state variable does not
+        suport addition, such as it contains angles.
+
     Attributes
     ----------
 
@@ -278,7 +284,8 @@ def residual(a, b):
     def __init__(self, dim_x, dim_z, dt, hx, fx, points,
                  sqrt_fn=None, x_mean_fn=None, z_mean_fn=None,
                  residual_x=None,
-                 residual_z=None):
+                 residual_z=None,
+                 state_add=None):
         """
         Create a Kalman filter. You are responsible for setting the
         various state variables to reasonable values; the defaults below will
@@ -327,6 +334,11 @@ def __init__(self, dim_x, dim_z, dt, hx, fx, points,
         else:
             self.residual_z = residual_z
 
+        if state_add is None:
+            self.state_add = np.add
+        else:
+            self.state_add = state_add
+
         # sigma points transformed through f(x) and h(x)
         # variables for efficiency so we don't recreate every update
 
@@ -465,7 +477,7 @@ def update(self, z, R=None, UT=None, hx=None, **hx_args):
         self.y = self.residual_z(z, zp)   # residual
 
         # update Gaussian state estimate (x, P)
-        self.x = self.x + dot(self.K, self.y)
+        self.x = self.state_add(self.x, dot(self.K, self.y))
         self.P = self.P - dot(self.K, dot(self.S, self.K.T))
 
         # save measurement and posterior state