Custom unscented_transform for rts #151

The RTS smoother of the UKF didn't let you specify a custom
unscented transform function.

Author: rlabbe

.gitignore

@@ -19,4 +19,5 @@ linux-64/
 osx-64/
 filterpy/kalman/i.py
 filterpy/inprogress.py
+.pytest_cache
 

filterpy/changelog.txt

@@ -1,9 +1,16 @@
+Version x.x.x
+=============
+* Improved documentation
+* Github #151 - added ability to specify unscented transform function for
+the UKF RTS smoother.
+
+
 Version 1.4.2
 =============
 
 * added a fast inverse of a diagonal matrix function common.inv_diagonal()
 
-* added ability to order Q matrix by dimension or derivative with the helper 
+* added ability to order Q matrix by dimension or derivative with the helper
 functions
 
 * mahalanobis, log-likelihood, and likelihood only computed when requested;

filterpy/kalman/UKF.py

@@ -611,7 +611,7 @@ def batch_filter(self, zs, Rs=None, dts=None, UT=None, saver=None):
 
         return (means, covariances)
 
-    def rts_smoother(self, Xs, Ps, Qs=None, dts=None):
+    def rts_smoother(self, Xs, Ps, Qs=None, dts=None, UT=None):
         """
         Runs the Rauch-Tung-Striebal Kalman smoother on a set of
         means and covariances computed by the UKF. The usual input
@@ -637,6 +637,12 @@ def rts_smoother(self, Xs, Ps, Qs=None, dts=None):
             an array, then each element k contains the time  at step k.
             Units are seconds.
 
+        UT : function(sigmas, Wm, Wc, noise_cov), optional
+            Optional function to compute the unscented transform for the sigma
+            points passed through hx. Typically the default function will
+            work - you can use x_mean_fn and z_mean_fn to alter the behavior
+            of the unscented transform.
+
         Returns
         -------
 
@@ -674,6 +680,9 @@ def rts_smoother(self, Xs, Ps, Qs=None, dts=None):
         if Qs is None:
             Qs = [self.Q] * n
 
+        if UT is None:
+            UT = unscented_transform
+
         # smoother gain
         Ks = zeros((n, dim_x, dim_x))
 
@@ -688,7 +697,7 @@ def rts_smoother(self, Xs, Ps, Qs=None, dts=None):
             for i in range(num_sigmas):
                 sigmas_f[i] = self.fx(sigmas[i], dts[k])
 
-            xb, Pb = unscented_transform(
+            xb, Pb = UT(
                 sigmas_f, self.Wm, self.Wc, self.Q,
                 self.x_mean, self.residual_x)