Merge pull request #198 from florisvb/circular-variable-support

Clean up circular variable support in rtsdiff

Author: pavelkomarov

README.md

@@ -24,7 +24,7 @@ Python methods for numerical differentiation of noisy data, including multi-obje
 
 ## Introduction
 
-PyNumDiff is a Python package that implements various methods for computing numerical derivatives of noisy data, which can be a critical step in developing dynamic models or designing control. There are seven different families of methods implemented in this repository:
+PyNumDiff is a Python package that implements many methods for computing numerical derivatives and smooth estimates of noisy data, which can be a critical step in developing dynamic models or designing control. There are seven different families of methods implemented in this repository:
 
 1. prefiltering followed by finite difference calculation
 2. iterated finite differencing
@@ -34,9 +34,11 @@ PyNumDiff is a Python package that implements various methods for computing nume
 6. generalized Kalman smoothing
 7. local approximation with linear model
 
-For a full list, explore modules in the [Sphinx documentation](https://pynumdiff.readthedocs.io/master/), or read section 7 of our [Taxonomy Paper](https://arxiv.org/abs/2512.09090).
+All are ultimately smoothing with similar runtime and accuracy, but some have situational advantages over others: For example, `robustdiff` is specialized to handle outliers; `splinediff`, `polydiff`, `rtsdiff`, and `robustdiff` can handle missing data; `splinediff`, `polydiff`, `rbfdiff`, `rtsdiff`, and `robustdiff` can handle irregularly-spaced data; and `rtsdiff` can handle inputs on a wrapped domain, like angles. All methods can accept blocks of multidimensional data, differentiating all vectors along the dimension given by the `axis` parameter.
 
-Most of these methods have multiple parameters, so we take a principled approach and propose a multi-objective optimization framework for choosing parameters that minimize a loss function to balance the faithfulness and smoothness of the derivative estimate. For more details, refer to [this paper](https://doi.org/10.1109/ACCESS.2020.3034077).
+For a full list and comparison, see section 7 of our [Taxonomy Paper](https://arxiv.org/abs/2512.09090) and explore modules in the [Sphinx documentation](https://pynumdiff.readthedocs.io/master/).
+
+All methods have hyperparameters, so we take a principled approach and propose a multi-objective optimization framework for choosing settings that minimize a loss function to balance the faithfulness and smoothness of the derivative estimate. For more details, refer to [this paper](https://doi.org/10.1109/ACCESS.2020.3034077).
 
 ## Installing
 

notebooks/7_circular_domain.ipynb

@@ -7,4 +7,5 @@
 | [Automatic Method Suggestion](https://github.com/florisvb/PyNumDiff/blob/master/notebooks/3_automatic_method_suggestion.ipynb) | A short demo of how to allow `pynumdiff` to choose a differentiation method for your data. |
 | [Performance Analysis](https://github.com/florisvb/PyNumDiff/blob/master/notebooks/4_performance_analysis.ipynb) | Experiments to compare methods' accuracy and bias across simulations. |
 | [Robustness to Outliers Demo](https://github.com/florisvb/PyNumDiff/blob/master/notebooks/5_robust_outliers_demo.ipynb) | This notebook shows a head-to-head of `RTSDiff`'s and `RobustDiff`'s minimum-RMSE performances on simulations with outliers, to illustrate the value of using a Huber loss in the Kalman MAP problem. |
-| [Multidimensionality Demo](https://github.com/florisvb/PyNumDiff/blob/master/notebooks/6_multidimensionality_demo.ipynb) | Demonstration of differentating multidimensional data along particular axes. |
+| [Multidimensionality Demo](https://github.com/florisvb/PyNumDiff/blob/master/notebooks/6_multidimensionality_demo.ipynb) | Demonstration of differentating multidimensional data along particular axes. |
+| [Circular Domain](https://github.com/florisvb/PyNumDiff/blob/master/notebooks/7_circular_domain.ipynb) | Demonstrates improved handling of data on a wrapped domain (e.g. angles) using `RTSDiff` with specialized innovation distance function (inside the Kalman filter). |

notebooks/7_circular_variables.ipynb

@@ -5,11 +5,10 @@
 try: import cvxpy
 except ImportError: pass
 
-from pynumdiff.utils.utility import huber_const, wrap_angle, ensure_iterable
+from pynumdiff.utils.utility import huber_const
 
 
-def kalman_filter(y, xhat0, P0, A, Q, C, R, B=None, u=None, save_P=True, 
-                  circular_vars=None, circular_units='rad'):
+def kalman_filter(y, xhat0, P0, A, Q, C, R, B=None, u=None, save_P=True, innovation_fn=None):
     """Run the forward pass of a Kalman filter. Expects discrete-time matrices; use :func:`scipy.linalg.expm`
     in the caller to convert from continuous time if needed.
 
@@ -25,10 +24,9 @@ def kalman_filter(y, xhat0, P0, A, Q, C, R, B=None, u=None, save_P=True,
     :param np.array u: optional control inputs, stacked in the direction of axis 0
     :param bool save_P: whether to save history of error covariance and a priori state estimates, used with rts
         smoothing but nonstandard to compute for ordinary filtering
-    :param bool or None circular_vars: bool indicating whether the measurement y is a circular (angular) variable 
-        that is wrapped. This will use a circular innovation calculation for the Kalman filter. The smoothed result
-        will be returned in an unwrapped form. 
-    :param string circular_units: 'rad' or 'deg' to specify whether wrapping is in degrees or radians. 
+    :param callable innovation_fn: optional function taking measurements and predicted measurements and returning the innovation.
+        When :code:`None`, traditional subtraction is used. This is primarily exposed to handle angles, which have a wrapped domain,
+        so alternative displacement measure :code:`lambda y, pred: (y - pred + np.pi) % (2*np.pi) - np.pi` is more appropriate.
 
     :return: - **xhat_pre** (np.array) -- a priori estimates of xhat, with axis=0 the batch dimension, so xhat[n] gets the nth step
              - **xhat_post** (np.array) -- a posteriori estimates of xhat
@@ -62,9 +60,7 @@ def kalman_filter(y, xhat0, P0, A, Q, C, R, B=None, u=None, save_P=True,
         P = P_.copy()
         if not np.isnan(y[n]): # handle missing data
             K = P_ @ C.T @ np.linalg.inv(C @ P_ @ C.T + R)
-            innovation = y[n] - C @ xhat_
-            if circular_vars is not None and circular_vars is not False:
-                innovation[0] = wrap_angle(innovation[0], circular_units)
+            innovation = y[n] - C @ xhat_ if innovation_fn is None else innovation_fn(y[n], C @ xhat_)
             xhat += K @ innovation
             P -= K @ C @ P_
         # the [n]th index of pre variables holds _{n|n-1} info; the [n]th index of post variables holds _{n|n} info
@@ -102,8 +98,7 @@ def rts_smooth(A, xhat_pre, xhat_post, P_pre, P_post, compute_P_smooth=True):
     return xhat_smooth if not compute_P_smooth else (xhat_smooth, P_smooth)
 
 
-def rtsdiff(x, dt_or_t, order, log_qr_ratio, forwardbackward, axis=0, 
-            circular_vars=None, circular_units='rad'):
+def rtsdiff(x, dt_or_t, order, log_qr_ratio, forwardbackward=False, axis=0, circular=False):
     """Perform Rauch-Tung-Striebel smoothing with a naive constant derivative model. Makes use of :code:`kalman_filter`
     and :code:`rts_smooth`, which are made public. :code:`constant_X` methods in this module call this function.
 
@@ -118,24 +113,16 @@ def rtsdiff(x, dt_or_t, order, log_qr_ratio, forwardbackward, axis=0,
     :param bool forwardbackward: indicates whether to run smoother forwards and backwards
         (usually achieves better estimate at end points)
     :param int axis: data dimension along which differentiation is performed
-    :param list[bool] circular_vars: set list element to bool for any axes of x that are a circular (angular) variable 
-        that is wrapped. This will use a circular innovation calculation for the Kalman filter. The smoothed result
-        will be returned in an unwrapped form. 
-    :param string circular_units: 'rad' or 'deg' to specify whether wrapping is in degrees or radians. 
+    :param bool circular: if :code:`True`, treat the measured quantity as a circular variable in radians, wrapping the
+        innovation to :math:`[-\\pi, \\pi]`. The input :code:`x` must be in radians; convert degrees with :code:`np.deg2rad`.
 
-    :return: - **x_hat** (np.array) -- estimated (smoothed) x, same shape as input :code:`x`
+    :return: - **x_hat** (np.array) -- estimated (smoothed) x, same shape as input :code:`x`.
+             When :code:`circular=True`, wrapped to :math:`[-\\pi, \\pi]`.
              - **dxdt_hat** (np.array) -- estimated derivative of x, same shape as input :code:`x`
     """
     N = x.shape[axis]
     if not np.isscalar(dt_or_t) and N != len(dt_or_t):
         raise ValueError("If `dt_or_t` is given as array-like, must have same length as x along `axis`.")
-    
-    # turn circular_vars into something with the same shape as the number of differentiated axes in x
-    if len(x.shape) > 1:
-        n = int(np.prod(x.shape[:axis] + x.shape[axis+1:]))
-        circular_vars = ensure_iterable(circular_vars, n)
-    else:
-        circular_vars = ensure_iterable(circular_vars, 1)
 
     q = 10**int(log_qr_ratio/2) # even-ish split of the powers across 0
     r = q/(10**log_qr_ratio)
@@ -160,29 +147,31 @@ def rtsdiff(x, dt_or_t, order, log_qr_ratio, forwardbackward, axis=0,
             Q_d[n] = eM[:order+1, order+1:] @ A_d[n].T
         if forwardbackward: A_d_bwd = np.linalg.inv(A_d[::-1]) # properly broadcasts, taking inv of each stacked 2D array
 
+    innovation_fn = (lambda y, pred: (y - pred + np.pi) % (2*np.pi) - np.pi) if circular else None # optionally wrap innovation to [-pi, pi], see #178
+
     x_hat = np.empty_like(x); dxdt_hat = np.empty_like(x)
     if forwardbackward: w = np.linspace(0, 1, N) # weights used to combine forward and backward results
 
-    for i, vec_idx in enumerate(np.ndindex(x.shape[:axis] + x.shape[axis+1:])): # works properly for 1D case too
+    for vec_idx in np.ndindex(x.shape[:axis] + x.shape[axis+1:]): # works properly for 1D case too
         s = vec_idx[:axis] + (slice(None),) + vec_idx[axis:] # for indexing the vector we wish to differentiate
-        xhat0 = np.zeros(order+1); xhat0[0] = x[s][0] if not np.isnan(x[s][0]) else 0 # The first estimate is the first seen state. See #110
+        xhat0 = np.zeros(order+1)
+        if not np.isnan(x[s][0]): xhat0[0] = x[s][0] # The first estimate is the first seen state. See #110
 
-        xhat_pre, xhat_post, P_pre, P_post = kalman_filter(x[s], xhat0, P0, A_d, Q_d, C, R, 
-                                                           circular_vars=circular_vars[i], circular_units=circular_units)
+        xhat_pre, xhat_post, P_pre, P_post = kalman_filter(x[s], xhat0, P0, A_d, Q_d, C, R, innovation_fn=innovation_fn)
         xhat_smooth = rts_smooth(A_d, xhat_pre, xhat_post, P_pre, P_post, compute_P_smooth=False)
         x_hat[s] = xhat_smooth[:,0] # first dimension is time, so slice first and second states at all times
         dxdt_hat[s] = xhat_smooth[:,1]
 
         if forwardbackward:
-            xhat0[0] = x[s][-1] if not np.isnan(x[s][-1]) else 0
+            if not np.isnan(x[s][-1]): xhat0[0] = x[s][-1]
             xhat_pre, xhat_post, P_pre, P_post = kalman_filter(x[s][::-1], xhat0, P0, A_d_bwd,
-                Q_d if Q_d.ndim == 2 else Q_d[::-1], C, R, 
-                circular_vars=circular_vars[i], circular_units=circular_units) # Use same Q matrices as before, because noise should still grow in reverse time
+                Q_d if Q_d.ndim == 2 else Q_d[::-1], C, R, innovation_fn=innovation_fn) # Use same Q matrices as before, because noise should still grow in reverse time
             xhat_smooth = rts_smooth(A_d_bwd, xhat_pre, xhat_post, P_pre, P_post, compute_P_smooth=False)
 
             x_hat[s] = x_hat[s] * w + xhat_smooth[:, 0][::-1] * (1-w)
             dxdt_hat[s] = dxdt_hat[s] * w + xhat_smooth[:, 1][::-1] * (1-w)
 
+    if circular: x_hat = (x_hat + np.pi) % (2*np.pi) - np.pi # wrap output to match the input domain, see #178
     return x_hat, dxdt_hat
 
 

notebooks/README.md

@@ -405,6 +405,36 @@ def test_multidimensionality(multidim_method_and_params, request):
         legend = ax3.legend(bbox_to_anchor=(0.7, 0.8)); legend.legend_handles[0].set_facecolor(pyplot.cm.viridis(0.6))
         fig.suptitle(f'{diff_method.__name__}', fontsize=16)
 
+def test_circular_rtsdiff(request):
+    """Ensure rtsdiff with circular=True correctly differentiates a wrapping angle signal in radians"""
+    dthdt = 5 # constant angular velocity in rad/s
+    th = dthdt * t # linearly increasing angle, crosses 2*pi boundaries
+    th_noisy = np.angle(np.exp(1j * (th + noise))) # add noise and wrap to [-pi, pi]
+
+    th_hat_naive, dthdt_hat_naive = rtsdiff(th_noisy, dt, order=1, log_qr_ratio=1, circular=False)
+    th_hat, dthdt_hat = rtsdiff(th_noisy, dt, order=1, log_qr_ratio=1, circular=True)
+    
+    naive_rmse = np.sqrt(np.mean((dthdt_hat_naive - dthdt)**2))
+    wrapped_rmse = np.sqrt(np.mean((dthdt_hat - dthdt)**2))
+    assert wrapped_rmse < naive_rmse
+
+    if request.config.getoption("--plot"):
+        from matplotlib import pyplot
+        fig, (ax1, ax2) = pyplot.subplots(2, 1, figsize=(10, 6), sharex=True)
+        ax1.plot(t, th_noisy, 'k+', label=r'$\theta$ noisy (wrapped)')
+        ax1.plot(t, th_hat_naive, 'C1--', label=r'$\hat{\theta}$ with circular=False')
+        ax1.plot(t, th_hat, 'C0', label=r'$\hat{\theta}$ with circular=True')
+        ax1.set_ylabel(r'$\theta$ (rad)')
+        ax1.legend()
+        ax2.axhline(dthdt, color='C2', xmin=0.045, xmax=0.955, label=r'true $\dot{\theta}$')
+        ax2.plot(t, dthdt_hat_naive, 'C1--', label=r'$\hat{\dot{\theta}}$ circular=False')
+        ax2.plot(t, dthdt_hat, 'C0', label=r'$\hat{\dot{\theta}}$ circular=True')
+        ax2.set_ylabel(r'$\dot{\theta}$ (rad/time)')
+        ax2.set_xlabel('t')
+        ax2.legend()
+        fig.suptitle('rtsdiff with circular domain', fontsize=16)
+
+
 # List of methods that can handle missing values
 nan_methods_and_params = [
     (splinediff, {'degree': 5, 's': 2}),

pynumdiff/kalman_smooth.py

@@ -202,51 +202,3 @@ def peakdet(x, delta, t=None):
     return np.array(maxtab), np.array(mintab)
 
 
-def wrap_angle(angle, units='rad', range='symmetric'):
-    """Wrap an angle to a specified range.
-    
-    :param np.array angle: angular values
-    :param string units: either 'rad' or 'deg'
-    :param string range: either 'symmetric' for [-pi, pi] / [-180, 180],
-                         or 'positive' for [0, 2pi] / [0, 360]
-
-    :return: - **angle** -- the angular values wrapped as requested
-    """
-    if units == 'rad':
-        period = 2 * np.pi
-        if range == 'symmetric':
-            return (angle + np.pi) % period - np.pi
-        elif range == 'positive':
-            return angle % period
-        else:
-            raise ValueError(f"Invalid range '{range}'. Expected 'symmetric' or 'positive'.")
-
-    elif units == 'deg':
-        period = 360.
-        if range == 'symmetric':
-            return (angle + 180.) % period - 180.
-        elif range == 'positive':
-            return angle % period
-        else:
-            raise ValueError(f"Invalid range '{range}'. Expected 'symmetric' or 'positive'.")
-
-    else:
-        raise ValueError(f"Invalid units '{units}'. Expected 'rad' or 'deg'.")
-    
-
-def ensure_iterable(v, length):
-    """Ensure v is a list of the specified length.
-
-    If v is not iterable (e.g. a scalar), it is broadcast
-    into a list by repeating it `length` times. If it is already iterable,
-    it is returned as-is.
-
-    :param v: a scalar or iterable
-    :param int length: desired length of the output list when broadcasting a scalar
-    :return: v repeated `length` times if scalar, otherwise unchanged
-
-    :return: - **v** -- list or iterable
-    """
-    if not hasattr(v, '__iter__'):
-        return [v] * length
-    return v