robustdiff to address #97

docs/source/kalman_smooth.rst

@@ -5,8 +5,10 @@ kalman_smooth
     :no-members:
 
 .. autofunction:: pynumdiff.kalman_smooth.rtsdiff
+.. autofunction:: pynumdiff.kalman_smooth.robustdiff
 .. autofunction:: pynumdiff.kalman_smooth.constant_velocity
 .. autofunction:: pynumdiff.kalman_smooth.constant_acceleration
 .. autofunction:: pynumdiff.kalman_smooth.constant_jerk
 .. autofunction:: pynumdiff.kalman_smooth.kalman_filter
-.. autofunction:: pynumdiff.kalman_smooth.rts_smooth
+.. autofunction:: pynumdiff.kalman_smooth.rts_smooth
+.. autofunction:: pynumdiff.kalman_smooth.convex_smooth

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):
@@ -106,7 +109,7 @@ def rts_smooth(_t, A, xhat_pre, xhat_post, P_pre, P_post, compute_P_smooth=True)
 
 def rtsdiff(x, _t, order, qr_ratio, forwardbackward):
     """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. Other constant derivative methods in this module call this function.
+    and :code:`rts_smooth`, which are made public. :code:`constant_X` methods in this module call this function.
 
     :param np.array[float] x: data series to differentiate
     :param float or array[float] _t: This function supports variable step size. This parameter is either the constant
@@ -251,3 +254,75 @@ 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, huberM=0):
+    """Perform outlier-robust differentiation by solving the MAP optimization problem:
+    :math:`\\min_{\\{x_n\\}} \\sum_{n=0}^{N-1} V(R^{-1/2}(y_n - C x_n)) + \\sum_{n=1}^{N-1} J(Q^{-1/2}(x_n - A x_{n-1}))`
+    with loss functions :math:`V,J` the :math:`\\ell_1` norm or Huber loss instead of the :math:`\\ell_2` norm
+    optimized by RTS smoothing. Uses convex optimization, calls :code:`convex_smooth`.
+
+    :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`.
+    :param float huberM: radius where quadratic of Huber loss function turns linear. M=0 reduces to the :math:`\\ell_1` norm.
+
+    :return: tuple[np.array, np.array] of\n
+        - **x_hat** -- estimated (smoothed) x
+        - **dxdt_hat** -- estimated derivative of x
+    """
+    q = 1e4 # I found q too small worsened condition number of the Q matrix, so fixing it at a biggish value
+    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) # Note this could handle variable dt, similar to rtsdiff
+    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-12 # for numerical stability with convex solver. Doesn't change answers appreciably (or at all).
+
+    x_states = convex_smooth(x, A_d, Q_d, R, C, huberM=huberM) # outsource solution of the convex optimization problem
+    return x_states[:, 0], x_states[:, 1]
+
+
+def convex_smooth(y, A, Q, R, C, huberM=0):
+    """Solve the optimization problem for robust smoothing using CVXPY. Note this currently assumes constant dt
+    but could be extended to handle variable step sizes by finding discrete-time A and Q for requisite gaps.
+
+    :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 < 1e-3 # Measurement terms: sum of ||R^(-1/2)(y_n - C x_n)||_1
+                     else cvxpy.sum(cvxpy.huber(R_sqrt_inv @ (y[n] - C @ x_states[n]), huberM)) for n in range(N)]) 
+    objective += cvxpy.sum([cvxpy.norm(Q_sqrt_inv @ (x_states[n] - A @ x_states[n-1]), 1) if huberM < 1e-3 # Process terms: sum of ||Q^(-1/2)(x_n - A x_{n-1})||_1
+                      else cvxpy.sum(cvxpy.huber(Q_sqrt_inv @ (x_states[n] - A @ x_states[n-1]), huberM)) for n in range(1, N)])
+
+    problem = cvxpy.Problem(cvxpy.Minimize(objective))
+    try:
+        problem.solve(solver=cvxpy.CLARABEL)
+    except cvxpy.error.SolverError:
+        warn(f"CLARABEL failed. Retrying with SCS.")
+        problem.solve(solver=cvxpy.SCS) # SCS is a lot slower but pretty bulletproof even with big condition numbers
+
+    if x_states.value is None: # There is occasional solver failure with huber as opposed to 1-norm
+        warn("Convex solvers failed with status {problem.status}. Returning NaNs.")
+        return np.full((N, A.shape[0]), np.nan)
+
+    return x_states.value