Merge pull request #166 from florisvb/improve-robust

Incrementing robust with different cost function

Author: pavelkomarov

pynumdiff/finite_difference/_finite_difference.py

@@ -4,7 +4,7 @@
 from warnings import warn
 
 
-def finitediff(x, dt, num_iterations, order):
+def finitediff(x, dt, num_iterations=1, order=2):
     """Perform iterated finite difference of a given order. This serves as the common backing function for
     all other methods in this module.
     

pynumdiff/kalman_smooth/_kalman_smooth.py

@@ -53,7 +53,7 @@ def kalman_filter(y, _t, xhat0, P0, A, Q, C, R, B=None, u=None, save_P=True):
         else:
             if not equispaced:
                 dt = _t[n] - _t[n-1]
-                eM = expm(M * dt) # form discrete-time matrices TODO doesn't work at n=0
+                eM = expm(M * dt) # form discrete-time matrices
                 An = eM[:m,:m] # upper left block
                 Qn = eM[:m,m:] @ An.T # upper right block
                 if dt < 0: Qn = np.abs(Qn) # eigenvalues go negative if reverse time, but noise shouldn't shrink
@@ -314,20 +314,20 @@ def convex_smooth(y, A, Q, C, R, huberM=0):
     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)])
-    
+    R_sqrt_inv = np.linalg.inv(sqrtm(R))
+    # Process terms: sum of 1/2||Q^(-1/2)(x_n - A x_{n-1})||_2^2
+    objective = 0.5*cvxpy.sum([cvxpy.sum_squares(Q_sqrt_inv @ (x_states[n] - A @ x_states[n-1])) for n in range(1, N)])
+    # Measurement terms: sum of sqrt(2)||R^(-1/2)(y_n - C x_n)||_1, per https://jmlr.org/papers/volume14/aravkin13a/aravkin13a.pdf section 6
+    objective += np.sqrt(2)*cvxpy.sum([cvxpy.norm(R_sqrt_inv @ (y[n] - C @ x_states[n]), 1) if huberM < 1e-3
+                     else cvxpy.sum(cvxpy.huber(R_sqrt_inv @ (y[n] - C @ x_states[n]), huberM)) for n in range(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)

pynumdiff/tests/test_diff_methods.py

@@ -51,7 +51,7 @@ def spline_irreg_step(*args, **kwargs): return splinediff(*args, **kwargs)
     (constant_acceleration, {'r':1e-3, 'q':1e4}), (constant_acceleration, [1e-3, 1e4]),
     (constant_jerk, {'r':1e-4, 'q':1e5}), (constant_jerk, [1e-4, 1e5]),
     (rtsdiff, {'order':2, 'qr_ratio':1e7, 'forwardbackward':True}),
-    (robustdiff, {'order':3, 'qr_ratio':1e6}),
+    (robustdiff, {'order':3, 'qr_ratio':1e8}),
     (velocity, {'gamma':0.5}), (velocity, [0.5]),
     (acceleration, {'gamma':1}), (acceleration, [1]),
     (jerk, {'gamma':10}), (jerk, [10]),
@@ -223,11 +223,11 @@ 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: [[(-15, -15), (-14, -14), (0, -1), (0, 0)],
-                 [(-14, -14), (-13, -14), (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)],
+    robustdiff: [[(-14, -15), (-17, -17), (0, -1), (1, 1)],
+                 [(-14, -14), (-13, -13), (0, -1), (1, 1)],
+                 [(-14, -14), (-13, -13), (0, -1), (1, 1)],
+                 [(-12, -12), (-2, -2), (0, -1), (1, 1)],
+                 [(0, 0), (2, 2), (0, 0), (2, 2)],
                  [(1, 1), (3, 3), (1, 1), (3, 3)]],
     lineardiff: [[(-6, -6), (-5, -6), (0, -1), (0, 0)],
                  [(0, 0), (2, 1), (0, 0), (2, 1)],

pynumdiff/utils/simulate.py

@@ -6,7 +6,7 @@
 
 # local imports
 from pynumdiff.utils.utility import peakdet
-from pynumdiff.finite_difference import second_order as finite_difference
+from pynumdiff.finite_difference import finitediff
 
 
 # pylint: disable-msg=too-many-locals, too-many-arguments, no-member
@@ -108,7 +108,7 @@ def triangle(duration=4, noise_type='normal', noise_parameters=(0, 0.5), outlier
     reversal_ts = t[reversal_idxs]
 
     pos = np.interp(t, reversal_ts, reversal_vals)
-    _, vel = finite_difference(pos, dt=simdt)
+    _, vel = finitediff(pos, dt=simdt)
     noisy_pos = _add_noise(pos, random_seed, noise_type, noise_parameters, outliers)
 
     idx = np.arange(0, len(t), int(dt/simdt))
@@ -182,7 +182,7 @@ def linear_autonomous(duration=4, noise_type='normal', noise_parameters=(0, 0.5)
     xs = np.vstack(xs).T
     pos = xs[0,:]
 
-    smooth_pos, vel = finite_difference(pos, simdt)
+    smooth_pos, vel = finitediff(pos, simdt)
     noisy_pos = _add_noise(pos, random_seed, noise_type, noise_parameters, outliers)
 
     idx = slice(0, len(t), int(dt/simdt)) # downsample so things are dt apart