moved huber_const to the utilities and realized my Huber scale identity in the robustdiff docstring was wrong and that there is a simpler way to write scaled Huber's throughout the repo, which incidentally also helps guard against divide by zeros

Author: pavelkomarov

pynumdiff/kalman_smooth/_kalman_smooth.py

@@ -3,10 +3,11 @@
 from scipy.linalg import expm, sqrtm
 from scipy.stats import norm
 from time import time
-
 try: import cvxpy
 except ImportError: pass
 
+from pynumdiff.utils.utility import huber_const
+
 
 def kalman_filter(y, dt_or_t, xhat0, P0, A, Q, C, R, B=None, u=None, save_P=True):
     """Run the forward pass of a Kalman filter, with regular or irregular step size.
@@ -278,7 +279,7 @@ def robustdiff(x, dt, order, log_q, log_r, proc_huberM=6, meas_huberM=0):
     proper :math:`\\ell_1` normalization.
 
     Note that :code:`log_q` and :code:`proc_huberM` are coupled, as are :code:`log_r` and :code:`meas_huberM`, via the relation
-    :math:`\\text{Huber}(q^{-1/2}v, M) = q^{-1}\\text{Huber}(v, Mq^{-1/2})`, but these are still independent enough that for
+    :math:`\\text{Huber}(q^{-1/2}v, M) = q^{-1}\\text{Huber}(v, Mq^{1/2})`, but these are still independent enough that for
     the purposes of optimization we cannot collapse them. Nor can :code:`log_q` and :code:`log_r` be combined into
     :code:`log_qr_ratio` as in RTS smoothing without the addition of a new absolute scale parameter, becausee :math:`q` and
     :math:`r` interact with the distinct Huber :math:`M` parameters.
@@ -329,11 +330,6 @@ def convex_smooth(y, A, Q, C, R, B=None, u=None, proc_huberM=6, meas_huberM=0):
     x_states = cvxpy.Variable((A.shape[0], N)) # each column is [position, velocity, acceleration, ...] at step n
     control = isinstance(B, np.ndarray) and isinstance(u, np.ndarray) # whether there is a control input
 
-    def huber_const(M): # from https://jmlr.org/papers/volume14/aravkin13a/aravkin13a.pdf, with correction for missing sqrt
-        a = 2*np.exp(-M**2 / 2)/M # huber_const smoothly transitions Huber between 1-norm and 2-norm squared cases
-        b = np.sqrt(2*np.pi)*(2*norm.cdf(M) - 1)
-        return np.sqrt((2*a*(1 + M**2)/M**2 + b)/(a + b))
-
     # It is extremely important to run time that CVXPY expressions be in vectorized form
     proc_resids = np.linalg.inv(sqrtm(Q)) @ (x_states[:,1:] - A @ x_states[:,:-1] - (0 if not control else B @ u[1:].T)) # all Q^(-1/2)(x_n - (A x_{n-1} + B u_n))
     meas_resids = np.linalg.inv(sqrtm(R)) @ (y.reshape(C.shape[0],-1) - C @ x_states) # all R^(-1/2)(y_n - C x_n)

pynumdiff/utils/evaluate.py

@@ -93,11 +93,9 @@ def robust_rme(u, v, padding=0, M=6):
     """
     if padding == 'auto': padding = max(1, int(0.025*len(u)))
     s = slice(padding, len(u)-padding) # slice out data we want to measure
-    N = s.stop - s.start
 
     sigma = stats.median_abs_deviation(u[s] - v[s], scale='normal') # M is in units of this robust scatter metric
-    if sigma < 1e-6: sigma = 1 # guard against divide by zero
-    return np.sqrt(2*np.mean(utility.huber((u[s] - v[s])/sigma, M))) * sigma
+    return np.sqrt(2*np.mean(utility.huber(u[s] - v[s], M*sigma)))
 
 
 def rmse(u, v, padding=0):

pynumdiff/utils/utility.py

@@ -102,6 +102,13 @@ def huber(x, M):
     absx = np.abs(x)
     return np.where(absx <= M, 0.5*x**2, M*(absx - 0.5*M))
 
+def huber_const(M):
+    """Scale that makes :code:`sum(huber())` interpolate :math:`\\sqrt{2}\\|\\cdot\\|_1` and :math:`\\frac{1}{2}\\|\\cdot\\|_2^2`,
+    from https://jmlr.org/papers/volume14/aravkin13a/aravkin13a.pdf, with correction for missing sqrt"""
+    a = 2*np.exp(-M**2 / 2)/M
+    b = np.sqrt(2*np.pi)*(2*norm.cdf(M) - 1)
+    return np.sqrt((2*a*(1 + M**2)/M**2 + b)/(a + b))
+
 
 def integrate_dxdt_hat(dxdt_hat, dt_or_t):
     """Wrapper for scipy.integrate.cumulative_trapezoid. Use 0 as first value so lengths match, see #88.
@@ -136,8 +143,7 @@ def estimate_integration_constant(x, x_hat, M=6):
         return np.median(x - x_hat) # Solves the L1 distance minimization
     else:
         sigma = median_abs_deviation(x - x_hat, scale='normal') # M is in units of this robust scatter metric
-        if sigma < 1e-6: sigma = 1 # guard against divide by zero
-        return minimize(lambda x0: np.mean(huber((x - (x_hat+x0))/sigma, M)), # fn to minimize in 1st argument
+        return minimize(lambda x0: np.sum(huber(x - (x_hat+x0), M*sigma)), # fn to minimize in 1st argument
             0, method='SLSQP').x[0] # result is a vector, even if initial guess is just a scalar