robustdiff to address #97

pynumdiff/__init__.py

@@ -2,10 +2,19 @@
 """
 from ._version import __version__
 
+try: # cvxpy dependencies
+    import cvxpy
+except ImportError:
+    from warnings import warn
+    warn("tvrdiff, robustdiff, and lineardiff not available due to lack of convex solver. To use those, install CVXPY.")
+else: # executes if try is successful
+    from .total_variation_regularization import tvrdiff, velocity, acceleration, jerk, smooth_acceleration, jerk_sliding
+    from .kalman_smooth import robustdiff, convex_smooth
+    from .linear_model import lineardiff
+
 from .finite_difference import finitediff, first_order, second_order, fourth_order
 from .smooth_finite_difference import meandiff, mediandiff, gaussiandiff, friedrichsdiff, butterdiff
 from .polynomial_fit import splinediff, polydiff, savgoldiff
-from .total_variation_regularization import tvrdiff, velocity, acceleration, jerk, iterative_velocity, smooth_acceleration, jerk_sliding
-from .kalman_smooth import kalman_filter, rts_smooth, rtsdiff, constant_velocity, constant_acceleration, constant_jerk
 from .basis_fit import spectraldiff, rbfdiff
-from .linear_model import lineardiff
+from .total_variation_regularization import iterative_velocity
+from .kalman_smooth import kalman_filter, rts_smooth, rtsdiff, constant_velocity, constant_acceleration, constant_jerk

pynumdiff/kalman_smooth/__init__.py

@@ -1,3 +1,11 @@
-"""This module implements Kalman filters
+"""This module implements constant-derivative model-based smoothers based on Kalman filtering and its generalization.
 """
+try:
+    import cvxpy
+except:
+    from warnings import warn
+    warn("CVXPY is not installed; robustdiff and l1_solve not available.")
+else: # runs if try was successful
+    from ._kalman_smooth import robustdiff, convex_smooth
+
 from ._kalman_smooth import kalman_filter, rts_smooth, rtsdiff, constant_velocity, constant_acceleration, constant_jerk

pynumdiff/kalman_smooth/_kalman_smooth.py

@@ -1,6 +1,9 @@
 import numpy as np
 from warnings import warn
-from scipy.linalg import expm
+from scipy.linalg import expm, sqrtm
+
+try: import cvxpy
+except ImportError: pass
 
 
 def kalman_filter(y, _t, xhat0, P0, A, Q, C, R, B=None, u=None, save_P=True):
@@ -251,3 +254,83 @@ def constant_jerk(x, dt, params=None, options=None, r=None, q=None, forwardbackw
         raise ValueError("`q` and `r` must be given.")
 
     return rtsdiff(x, dt, 3, q/r, forwardbackward)
+
+
+def robustdiff(x, dt, order, qr_ratio):
+    """Perform robust differentiation using L1-norm optimization instead of L2-norm RTS smoothing.
+
+    This function solves the L1-normed MAP optimization problem for outlier-resistant differentiation:
+    min_{x_n} sum_{n=0}^{N-1} ||R^(-1/2)(y_n - C x_n)||_1 + sum_{n=1}^{N-1} ||Q^(-1/2)(x_n - A x_{n-1})||_1
+
+    The L1 norm provides better robustness to outliers compared to the L2 norm used in standard
+    Kalman smoothing. Uses CVXPY for convex optimization.
+
+    :param np.array[float] x: data series to differentiate
+    :param float dt: step size
+    :param int order: which derivative to stabilize in the constant-derivative model (1=velocity, 2=acceleration, 3=jerk)
+    :param float qr_ratio: the process noise level of the divided by the measurement noise level, because the result is
+        dependent on the relative rather than absolute size of :math:`q` and :math:`r`.
+
+    :return: tuple[np.array, np.array] of\n
+        - **x_hat** -- estimated (smoothed) x
+        - **dxdt_hat** -- estimated derivative of x
+    """
+    #q = 10**int(np.log10(qr_ratio)) # even-ish split of the powers across 0
+    #r = q/qr_ratio
+    q = 1e4
+    r = q/qr_ratio
+
+    A_c = np.diag(np.ones(order), 1) # continuous-time A just has 1s on the first diagonal (where 0th is main diagonal)
+    Q_c = np.zeros(A_c.shape); Q_c[-1,-1] = q # continuous-time uncertainty around the last derivative
+    C = np.zeros((1, order+1)); C[0,0] = 1 # we measure only y = noisy x
+    R = np.array([[r]]) # 1 observed state, so this is 1x1
+
+    # convert to discrete time using matrix exponential
+    eM = expm(np.block([[A_c, Q_c], [np.zeros(A_c.shape), -A_c.T]]) * dt)
+    A_d = eM[:order+1, :order+1]
+    Q_d = eM[:order+1, order+1:] @ A_d.T
+    if np.linalg.cond(Q_d) > 1e12: Q_d += np.eye(order + 1)*1e-15 # for numerical stability with convex solver. Doesn't change answers appreciably (or at all).
+
+    print(f"order = {order}, qr_ratio = {qr_ratio:.3e}, (q,r) = {(q,r)}, cond(Q_d) = {np.linalg.cond(Q_d):.3e}")
+
+    # Solve the L1-norm optimization problem
+    x_states = convex_smooth(x, A_d, Q_d, R, C)
+    return x_states[:, 0], x_states[:, 1]
+
+
+def convex_smooth(y, A, Q, R, C, huberM=1):
+    """Solve the L1-norm optimization problem for robust smoothing using CVXPY.
+
+    :param np.array[float] y: measurements
+    :param np.array A: discrete-time state transition matrix
+    :param np.array Q: discrete-time process noise covariance matrix
+    :param np.array R: measurement noise covariance matrix
+    :param np.array C: measurement matrix
+    :param float huberM: radius where quadratic of Huber loss function turns linear. M=0 reduces to the :math:`\\ell_1` norm.
+    :return: np.array -- state estimates (N x state_dim)
+    """
+    N = len(y)
+    x_states = cvxpy.Variable((N, A.shape[0])) # each row is [position, velocity, acceleration, ...] at step n
+
+    R_sqrt_inv = np.linalg.inv(sqrtm(R))
+    Q_sqrt_inv = np.linalg.inv(sqrtm(Q))
+    objective = cvxpy.sum([cvxpy.norm(R_sqrt_inv @ (y[n] - C @ x_states[n]), 1) if huberM == 0
+                     else cvxpy.sum(cvxpy.huber(R_sqrt_inv @ (y[n] - C @ x_states[n]), huberM)) for n in range(N)]) # Measurement terms: sum of |R^(-1/2)(y_n - C x_n)|_1
+    objective += cvxpy.sum([cvxpy.norm(Q_sqrt_inv @ (x_states[n] - A @ x_states[n-1]), 1) if huberM == 0
+                      else cvxpy.sum(cvxpy.huber(Q_sqrt_inv @ (x_states[n] - A @ x_states[n-1]), huberM)) for n in range(1, N)]) # Process terms: sum of |Q^(-1/2)(x_n - A x_{n-1})|_1
+
+    problem = cvxpy.Problem(cvxpy.Minimize(objective))
+    try:
+        problem.solve(solver=cvxpy.CLARABEL)
+    except cvxpy.error.SolverError as e1:
+        print(f"CLARABEL failed ({e1}). Retrying with SCS.")
+        try:
+            problem.solve(solver=cvxpy.SCS) # SCS is a lot slower but pretty bulletproof even with big condition numbers
+        except cvxpy.error.SolverError as e2:
+            print(f"SCS failed ({e2}). Returning NaNs.")
+
+    if x_states.value is None:
+        print(f"Solver returned None. Status: {problem.status}")
+        return np.full((N, A.shape[0]), np.nan)
+
+    return x_states.value

pynumdiff/optimize/_optimize.py

@@ -14,7 +14,7 @@
 from ..polynomial_fit import polydiff, savgoldiff, splinediff
 from ..basis_fit import spectraldiff, rbfdiff
 from ..total_variation_regularization import tvrdiff, velocity, acceleration, jerk, iterative_velocity, smooth_acceleration, jerk_sliding
-from ..kalman_smooth import rtsdiff, constant_velocity, constant_acceleration, constant_jerk
+from ..kalman_smooth import rtsdiff, constant_velocity, constant_acceleration, constant_jerk, robustdiff
 
 
 # Map from method -> (search_space, bounds_low_hi)
@@ -88,7 +88,10 @@
                          'q': [1e-8, 1e-4, 1e-1, 1e1, 1e4, 1e8],
                          'r': [1e-8, 1e-4, 1e-1, 1e1, 1e4, 1e8]},
                          {'q': (1e-10, 1e10),
-                          'r': (1e-10, 1e10)})
+                          'r': (1e-10, 1e10)}),
+    robustdiff: ({'order': {1, 2, 3}, #categorical
+                    'qr_ratio': [10**k for k in range(-1, 18, 3)]},
+                   {'qr_ratio': [1e-1, 1e18]})
 }
 for method in [second_order, fourth_order]:
     method_params_and_bounds[method] = method_params_and_bounds[first_order]
@@ -246,9 +249,9 @@ def suggest_method(x, dt, dxdt_truth=None, cutoff_frequency=None):
         spectraldiff, rbfdiff, splinediff, polydiff, savgoldiff, rtsdiff]
     try: # optionally skip some methods
         import cvxpy
-        methods += [tvrdiff, smooth_acceleration]
+        methods += [tvrdiff, smooth_acceleration, robustdiff]
     except ImportError:
-        warn("CVXPY not installed, skipping tvrdiff and smooth_acceleration")
+        warn("CVXPY not installed, skipping tvrdiff, smooth_acceleration, and robustdiff")
 
     best_value = float('inf') # core loop
     for func in tqdm(methods):

pynumdiff/tests/test_diff_methods.py

@@ -7,7 +7,7 @@
 from ..basis_fit import spectraldiff, rbfdiff
 from ..polynomial_fit import polydiff, savgoldiff, splinediff
 from ..total_variation_regularization import velocity, acceleration, jerk, iterative_velocity, smooth_acceleration, jerk_sliding
-from ..kalman_smooth import rtsdiff, constant_velocity, constant_acceleration, constant_jerk
+from ..kalman_smooth import rtsdiff, constant_velocity, constant_acceleration, constant_jerk, robustdiff
 from ..smooth_finite_difference import mediandiff, meandiff, gaussiandiff, friedrichsdiff, butterdiff
 # Function aliases for testing cases where parameters change the behavior in a big way, so error limits can be indexed in dict
 def iterated_second_order(*args, **kwargs): return second_order(*args, **kwargs)
@@ -59,6 +59,7 @@ def spline_irreg_step(*args, **kwargs): return splinediff(*args, **kwargs)
     (lineardiff, {'order':3, 'gamma':5, 'window_size':11, 'solver':'CLARABEL'}), (lineardiff, [3, 5, 11], {'solver':'CLARABEL'}),
     (rbfdiff, {'sigma':0.5, 'lmbd':0.001})
     ]
+diff_methods_and_params = [(robustdiff, {'order':3, 'qr_ratio':1e6})]
 
 # All the testing methodology follows the exact same pattern; the only thing that changes is the
 # closeness to the right answer various methods achieve with the given parameterizations and random seed.
@@ -210,6 +211,12 @@ def spline_irreg_step(*args, **kwargs): return splinediff(*args, **kwargs)
               [(-2, -3), (0, 0), (0, -1), (1, 1)],
               [(-1, -2), (1, 1), (0, -1), (1, 1)],
               [(0, 0), (3, 3), (0, 0), (3, 3)]],
+    robustdiff: [[(-25, -25), (-15, -15), (0, -1), (0, 0)],
+                 [(-14, -14), (-13, -13), (0, -1), (0, 0)],
+                 [(-14, -14), (-13, -13), (0, -1), (0, 0)],
+                 [(-1, -1), (0, 0), (0, -1), (1, 0)],
+                 [(0, 0), (1, 1), (0, 0), (1, 1)],
+                 [(1, 1), (3, 3), (1, 1), (3, 3)]],
     spectraldiff: [[(-9, -10), (-14, -15), (-1, -1), (0, 0)],
                    [(0, 0), (1, 1), (0, 0), (1, 1)],
                    [(1, 1), (1, 1), (1, 1), (1, 1)],

pynumdiff/total_variation_regularization/__init__.py

@@ -2,12 +2,12 @@
 """
 try:
     import cvxpy
-    from ._total_variation_regularization import tvrdiff, velocity, acceleration, jerk, jerk_sliding, smooth_acceleration
 except:
     from warnings import warn
     warn("Limited Total Variation Regularization Support Detected! CVXPY is not installed. " +
         "Many Total Variation Methods require CVXPY including: velocity, acceleration, jerk, jerk_sliding, smooth_acceleration. " +
-        "Please install CVXPY to use these methods. Recommended to also install MOSEK and obtain a MOSEK license. " +
-        "You can still use: total_variation_regularization.iterative_velocity.") 
+        "Please install CVXPY to use these methods. You can still use: total_variation_regularization.iterative_velocity.")
+else: # executes if try is successful
+    from ._total_variation_regularization import tvrdiff, velocity, acceleration, jerk, jerk_sliding, smooth_acceleration
 
 from ._total_variation_regularization import iterative_velocity