From a7685ccdfbedf35e885a3dd86d13946964cc255f Mon Sep 17 00:00:00 2001 From: pavelkomarov Date: Wed, 29 Oct 2025 16:52:17 -0700 Subject: [PATCH] made process term sum of squares, and balanced process and measurement terms, as in Aravkin2013 --- .../finite_difference/_finite_difference.py | 2 +- pynumdiff/kalman_smooth/_kalman_smooth.py | 16 ++++++++-------- pynumdiff/tests/test_diff_methods.py | 12 ++++++------ pynumdiff/utils/simulate.py | 6 +++--- 4 files changed, 18 insertions(+), 18 deletions(-) diff --git a/pynumdiff/finite_difference/_finite_difference.py b/pynumdiff/finite_difference/_finite_difference.py index 14453f8..59e8c2b 100644 --- a/pynumdiff/finite_difference/_finite_difference.py +++ b/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. diff --git a/pynumdiff/kalman_smooth/_kalman_smooth.py b/pynumdiff/kalman_smooth/_kalman_smooth.py index 3bbc87f..317de3a 100644 --- a/pynumdiff/kalman_smooth/_kalman_smooth.py +++ b/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) diff --git a/pynumdiff/tests/test_diff_methods.py b/pynumdiff/tests/test_diff_methods.py index d46e45a..26022d1 100644 --- a/pynumdiff/tests/test_diff_methods.py +++ b/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)], diff --git a/pynumdiff/utils/simulate.py b/pynumdiff/utils/simulate.py index c9fe880..e454df0 100644 --- a/pynumdiff/utils/simulate.py +++ b/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