From ecc25f27fb77ae210a7f6842b864923a3d78c09a Mon Sep 17 00:00:00 2001 From: Floris van Breugel Date: Tue, 27 Oct 2026 09:51:45 -0700 Subject: [PATCH] rtsdiff circular: choose the 2pi branch from the previous posterior (#240) With circular=True the innovation was wrapped against the filter's prediction. After a large one-step change in angle, a high-gain prediction can overshoot the measurement by more than pi, so the wrapped innovation takes the branch 2pi away and the filter locks onto an aliased solution whose derivative is off by, or grows by, multiples of 2pi/dt. kalman_filter gains circular=True, which keeps the wrapped innovation but picks its branch relative to the previous posterior: r + wrap(y - r) - pred. rtsdiff uses it by default and keeps the old behaviour as circular_branch='prediction', which still tracks fast, steady rotation through noise better at low gain. innovation_fn is unchanged, so this is backward compatible. Tests cover the issue's step and Drosophila examples, noise robustness at the issue's gain, equivalence with differentiating unwrapped data (irregular steps, NaNs, multidim), invariance to whole turns, and argument validation. Co-Authored-By: Claude Opus 5.5 --- pynumdiff/kalman_smooth.py | 36 ++++++++--- pynumdiff/tests/test_diff_methods.py | 95 ++++++++++++++++++++++++++-- 2 files changed, 118 insertions(+), 13 deletions(-) diff --git a/pynumdiff/kalman_smooth.py b/pynumdiff/kalman_smooth.py index a34da12..2822516 100644 --- a/pynumdiff/kalman_smooth.py +++ b/pynumdiff/kalman_smooth.py @@ -7,7 +7,7 @@ from pynumdiff.utils.utility import huber_const, robust_noise_scale -def kalman_filter(y, xhat0, P0, A, Q, C, R, B=None, u=None, save_P=True, innovation_fn=None): +def kalman_filter(y, xhat0, P0, A, Q, C, R, B=None, u=None, save_P=True, innovation_fn=None, circular=False): """Run the forward pass of a Kalman filter. This function is exposed publicly so users can create their own differentiator if they know their system model. Expects discrete-time matrices; use :func:`scipy.linalg.expm` in the caller to convert from continuous time if needed. @@ -25,8 +25,14 @@ def kalman_filter(y, xhat0, P0, A, Q, C, R, B=None, u=None, save_P=True, innovat :param bool save_P: whether to save history of error covariance and a priori state estimates, used with rts smoothing but nonstandard to compute for ordinary filtering :param callable innovation_fn: optional function taking measurements and predicted measurements and returning the innovation. - When :code:`None`, traditional subtraction is used. This is primarily exposed to handle angles, which have a wrapped domain, - so alternative displacement measure :code:`lambda y, pred: (y - pred + np.pi) % (2*np.pi) - np.pi` is more appropriate. + When :code:`None`, traditional subtraction is used. For angles, :code:`circular` is usually the better choice; wrapping + against the prediction instead, :code:`lambda y, pred: (y - pred + np.pi) % (2*np.pi) - np.pi`, can lock onto an aliased + solution when a high-gain filter overshoots by more than :math:`\\pi`, see `#240 `_. + Cannot be combined with :code:`circular`. + :param bool circular: if :code:`True`, treat measurements as angles in radians. The innovation is wrapped, but its + :math:`2\\pi` branch is chosen relative to the previous a posteriori measurement estimate :math:`r = C\\hat{x}_{n-1|n-1}` + rather than the prediction: :math:`r + \\text{wrap}(y_n - r) - C\\hat{x}_{n|n-1}`. Across missing data, :math:`r` + follows the model's prediction. :return: - **xhat_pre** (np.array) -- a priori estimates of xhat, with axis=0 the batch dimension, so xhat[n] gets the nth step - **xhat_post** (np.array) -- a posteriori estimates of xhat @@ -35,6 +41,7 @@ def kalman_filter(y, xhat0, P0, A, Q, C, R, B=None, u=None, save_P=True, innovat if :code:`save_P` is :code:`True` else only **xhat_post** to save memory """ if A.ndim != Q.ndim: raise ValueError("number of dimensions of A and Q must agree, either single matrix or stacked") + if circular and innovation_fn is not None: raise ValueError("`circular` computes its own innovation, so it cannot be combined with `innovation_fn`") N = y.shape[0] m = xhat0.shape[0] # dimension of the state @@ -47,7 +54,9 @@ def kalman_filter(y, xhat0, P0, A, Q, C, R, B=None, u=None, save_P=True, innovat control = isinstance(B, np.ndarray) and isinstance(u, np.ndarray) # whether there is a control input if A.ndim == 2: An, Qn, Bn = A, Q, B # single matrices, assign once outside the loop + xhat = xhat0 for n in range(N): + ref = C @ xhat # previous posterior, used to pick the 2pi branch if circular, because a high-gain prediction can overshoot by > pi. See #240 if n == 0: # first iteration is a special case, involving less work xhat_ = xhat0 P_ = P0 @@ -60,7 +69,8 @@ def kalman_filter(y, xhat0, P0, A, Q, C, R, B=None, u=None, save_P=True, innovat P = P_.copy() if not np.isnan(y[n]): # handle missing data K = P_ @ C.T @ np.linalg.inv(C @ P_ @ C.T + R) - innovation = y[n] - C @ xhat_ if innovation_fn is None else innovation_fn(y[n], C @ xhat_) + if circular: innovation = ref + (y[n] - ref + np.pi) % (2*np.pi) - np.pi - C @ xhat_ + else: innovation = y[n] - C @ xhat_ if innovation_fn is None else innovation_fn(y[n], C @ xhat_) xhat += K @ innovation P -= K @ C @ P_ # the [n]th index of pre variables holds _{n|n-1} info; the [n]th index of post variables holds _{n|n} info @@ -99,7 +109,7 @@ def rts_smooth(A, xhat_pre, xhat_post, P_pre, P_post, compute_P_smooth=True): return xhat_smooth if not compute_P_smooth else (xhat_smooth, P_smooth) -def rtsdiff(x, dt_or_t, order, log_qr_ratio, forwardbackward=False, axis=0, circular=False): +def rtsdiff(x, dt_or_t, order, log_qr_ratio, forwardbackward=False, axis=0, circular=False, circular_branch='posterior'): """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. @@ -116,12 +126,20 @@ def rtsdiff(x, dt_or_t, order, log_qr_ratio, forwardbackward=False, axis=0, circ :param int axis: data dimension along which differentiation is performed :param bool circular: if :code:`True`, treat the measured quantity as a circular variable in radians, wrapping the innovation to :math:`[-\\pi, \\pi]`. The input :code:`x` must be in radians; convert degrees with :code:`np.deg2rad`. + :param str circular_branch: only used if :code:`circular=True`. Wrapped data can't distinguish an angle from itself + plus whole turns, so each innovation's :math:`2\\pi` branch must be chosen relative to some reference. + :code:`'posterior'` (default) uses the previous filtered estimate, which is robust to sudden large swings and + to high :code:`log_qr_ratio`. :code:`'prediction'` uses the model's prediction, which tracks fast, steady + rotation through noise better at low gain, but can lock onto an aliased solution, with derivative off by + multiples of :math:`2\\pi/\\Delta t`, when a high-gain prediction overshoots by more than :math:`\\pi`. See + `#240 `_. :return: - **x_hat** (np.array) -- estimated (smoothed) x, same shape as input :code:`x`. When :code:`circular=True`, wrapped to :math:`[-\\pi, \\pi]`. - **dxdt_hat** (np.array) -- estimated derivative of x, same shape as input :code:`x` """ N = x.shape[axis] + if circular_branch not in ('posterior', 'prediction'): raise ValueError("`circular_branch` must be 'posterior' or 'prediction'.") if not np.isscalar(dt_or_t): if N != len(dt_or_t): raise ValueError("If `dt_or_t` is given as array-like, must have same length as x along `axis`.") if np.any(np.diff(dt_or_t) <= 0): raise ValueError("`dt_or_t` must be strictly increasing. Out-of-order or repeated " @@ -150,7 +168,9 @@ def rtsdiff(x, dt_or_t, order, log_qr_ratio, forwardbackward=False, axis=0, circ Q_d[n] = eM[:order+1, order+1:] @ A_d[n].T if forwardbackward: A_d_bwd = np.linalg.inv(A_d[::-1]) # properly broadcasts, taking inv of each stacked 2D array - innovation_fn = (lambda y, pred: (y - pred + np.pi) % (2*np.pi) - np.pi) if circular else None # optionally wrap innovation to [-pi, pi], see #178 + # optionally wrap innovation to [-pi, pi], see #178, choosing the 2pi branch from the previous posterior or the prediction, see #240 + wrapping = {'posterior': {'circular': True}, + 'prediction': {'innovation_fn': lambda y, pred: (y - pred + np.pi) % (2*np.pi) - np.pi}}[circular_branch] if circular else {} x_hat = np.empty(x.shape); dxdt_hat = np.empty(x.shape) # not empty_like, because integer type could be inherited and silently truncate if forwardbackward: w = np.linspace(0, 1, N) # weights used to combine forward and backward results @@ -160,7 +180,7 @@ def rtsdiff(x, dt_or_t, order, log_qr_ratio, forwardbackward=False, axis=0, circ xhat0 = np.zeros(order+1) if not np.isnan(x[s][0]): xhat0[0] = x[s][0] # The first estimate is the first seen state. See #110 - xhat_pre, xhat_post, P_pre, P_post = kalman_filter(x[s], xhat0, P0, A_d, Q_d, C, R, innovation_fn=innovation_fn) + xhat_pre, xhat_post, P_pre, P_post = kalman_filter(x[s], xhat0, P0, A_d, Q_d, C, R, **wrapping) xhat_smooth = rts_smooth(A_d, xhat_pre, xhat_post, P_pre, P_post, compute_P_smooth=False) x_hat[s] = xhat_smooth[:,0] # first dimension is time, so slice first and second states at all times dxdt_hat[s] = xhat_smooth[:,1] @@ -168,7 +188,7 @@ def rtsdiff(x, dt_or_t, order, log_qr_ratio, forwardbackward=False, axis=0, circ if forwardbackward: if not np.isnan(x[s][-1]): xhat0[0] = x[s][-1] xhat_pre, xhat_post, P_pre, P_post = kalman_filter(x[s][::-1], xhat0, P0, A_d_bwd, - Q_d if Q_d.ndim == 2 else Q_d[::-1], C, R, innovation_fn=innovation_fn) # Use same Q matrices as before, because noise should still grow in reverse time + Q_d if Q_d.ndim == 2 else Q_d[::-1], C, R, **wrapping) # Use same Q matrices as before, because noise should still grow in reverse time xhat_smooth = rts_smooth(A_d_bwd, xhat_pre, xhat_post, P_pre, P_post, compute_P_smooth=False) if circular: # forward and backward passes track unwrapped angle and can sit whole turns apart, so de-alias, see #208 diff --git a/pynumdiff/tests/test_diff_methods.py b/pynumdiff/tests/test_diff_methods.py index a550431..e337eca 100644 --- a/pynumdiff/tests/test_diff_methods.py +++ b/pynumdiff/tests/test_diff_methods.py @@ -1,13 +1,13 @@ """Unit tests of core differentiation functionality""" import numpy as np -from pytest import mark +from pytest import mark, raises from ..smooth_finite_difference import kerneldiff, butterdiff from ..finite_difference import finitediff from ..polynomial_fit import polydiff, savgoldiff, splinediff from ..basis_fit import spectraldiff, rbfdiff, waveletdiff from ..total_variation_regularization import iterative_velocity, smooth_acceleration, tvrdiff -from ..kalman_smooth import rtsdiff, robustdiff +from ..kalman_smooth import rtsdiff, robustdiff, kalman_filter from ..linear_model import lineardiff # Function aliases for testing cases where parameters change the behavior in a big way, so error limits can be indexed in dict def kerneldiff_uniform(*args, **kwargs): return kerneldiff(*args, **kwargs) @@ -440,15 +440,17 @@ def test_missing_data(diff_method_and_params): assert np.all(np.isfinite(dxdt_hat)) +@mark.parametrize("circular_branch", ["posterior", "prediction"]) @mark.parametrize("fwdbwd", [False, True]) -def test_circular_rtsdiff(request, fwdbwd): +def test_circular_rtsdiff(request, fwdbwd, circular_branch): """Ensure rtsdiff with circular=True correctly differentiates a wrapping angle signal in radians""" dthdt = 5 # constant angular velocity in rad/s th = dthdt * t # linearly increasing angle, crosses 2*pi boundaries th_noisy = np.angle(np.exp(1j * (th + noise))) # add noise and wrap to [-pi, pi] th_hat_naive, dthdt_hat_naive = rtsdiff(th_noisy, dt, order=1, log_qr_ratio=1, circular=False, forwardbackward=fwdbwd) - th_hat, dthdt_hat = rtsdiff(th_noisy, dt, order=1, log_qr_ratio=1, circular=True, forwardbackward=fwdbwd) + th_hat, dthdt_hat = rtsdiff(th_noisy, dt, order=1, log_qr_ratio=1, circular=True, forwardbackward=fwdbwd, + circular_branch=circular_branch) naive_rmse = np.sqrt(np.mean((dthdt_hat_naive - dthdt)**2)) wrapped_rmse = np.sqrt(np.mean((dthdt_hat - dthdt)**2)) @@ -471,4 +473,87 @@ def test_circular_rtsdiff(request, fwdbwd): ax2.set_ylabel(r'$\dot{\theta}$ (rad/time)') ax2.set_xlabel('t') ax2.legend() - fig.suptitle(f'rtsdiff with circular domain, forwardbackward={fwdbwd}', fontsize=16) + fig.suptitle(f'rtsdiff with circular domain, forwardbackward={fwdbwd}, circular_branch={circular_branch}', fontsize=16) + + +def angular_diff(a, b): + """Difference on the circle, so whole turns don't count""" + return np.angle(np.exp(1j*(a - b))) + +# Course direction (rad) of Drosophila trajectory 20121002_184808_2605 at 100 Hz, from #240. The 4.18 rad jump between +# samples 7 and 8 is a -2.10 rad swing after wrapping, which threw rtsdiff onto an aliased solution +fly_course = np.array([-2.265256, -2.269022, -2.284692, -2.315834, -2.362328, -2.422941, -2.503468, -2.691861, 1.486292, + 0.793781, 0.62091, 0.455346, 0.225893, -0.023637, -0.296044, -0.746268, -1.377319, -1.821492, -2.05377, -2.15313, -2.145599]) + +@mark.parametrize("fwdbwd", [False, True]) +@mark.parametrize("order, log_qr_ratio, step", [(2, 12, 2.0), (2, 14, 1.3), (1, 8, 3.0), (3, 14, 2.2)]) +def test_circular_step_no_aliasing(order, log_qr_ratio, step, fwdbwd): + """Ensure a large one-step change in angle, though < pi, doesn't throw a high-gain circular rtsdiff onto an aliased + solution with derivative off by multiples of 2pi/dt, see #240""" + th = np.r_[np.zeros(10), np.full(10, step)] # no wrap ever occurs, so circular=False gives the right answer + x_hat, dxdt_hat = rtsdiff(th, 0.01, order, log_qr_ratio, forwardbackward=fwdbwd, circular=True) + x_hat_plain, dxdt_hat_plain = rtsdiff(th, 0.01, order, log_qr_ratio, forwardbackward=fwdbwd) + + assert np.allclose(dxdt_hat, dxdt_hat_plain) + assert np.allclose(angular_diff(x_hat, x_hat_plain), 0) + + +@mark.parametrize("fwdbwd", [False, True]) +def test_circular_fly_trajectory(fwdbwd): + """Ensure the real trajectory from #240, at the settings the optimizer chose for it, matches differentiating the unwrapped data""" + x_hat, dxdt_hat = rtsdiff(fly_course, 0.01, 2, 12, forwardbackward=fwdbwd, circular=True) + x_hat_unw, dxdt_hat_unw = rtsdiff(np.unwrap(fly_course), 0.01, 2, 12, forwardbackward=fwdbwd) + + assert np.allclose(dxdt_hat, dxdt_hat_unw) + assert np.allclose(angular_diff(x_hat, x_hat_unw), 0) + + +@mark.parametrize("fwdbwd", [False, True]) +@mark.parametrize("scenario", ["uniform", "irregular", "missing", "multidim"]) +def test_circular_matches_unwrapped(scenario, fwdbwd): + """Ensure that when the angle moves less than pi per step, circular=True on wrapped data gives the same answer as + circular=False on the unwrapped data, including for variable step size, missing data, and multidimensional input""" + _t = t_irreg if scenario == "irregular" else t + th = 3*_t + 2*np.sin(2*_t) + noise; axis = 0 # crosses the +-pi boundary several times, moving < pi/2 per step + if scenario == "missing": th[[5, 10, 15]] = np.nan; th[22:26] = np.nan # point gaps and a contiguous run, as in test_missing_data + if scenario == "multidim": th = np.vstack([th, 0.5*th - 1, 2 - th]); axis = 1 + dt_or_t = t_irreg if scenario == "irregular" else dt + + x_hat, dxdt_hat = rtsdiff(np.angle(np.exp(1j*th)), dt_or_t, 2, 6, forwardbackward=fwdbwd, axis=axis, circular=True) + x_hat_plain, dxdt_hat_plain = rtsdiff(th, dt_or_t, 2, 6, forwardbackward=fwdbwd, axis=axis) + + assert np.allclose(dxdt_hat, dxdt_hat_plain) + assert np.allclose(angular_diff(x_hat, x_hat_plain), 0) + + +@mark.parametrize("circular_branch", ["posterior", "prediction"]) +def test_circular_invariant_to_whole_turns(circular_branch): + """Ensure adding whole turns to individual samples of circular data doesn't change the result""" + th = np.angle(np.exp(1j*(5*t + noise))) + turns = 2*np.pi*np.random.default_rng(0).integers(-3, 4, len(t)) + + x_hat, dxdt_hat = rtsdiff(th, dt, 2, 4, forwardbackward=True, circular=True, circular_branch=circular_branch) + x_hat_turned, dxdt_hat_turned = rtsdiff(th + turns, dt, 2, 4, forwardbackward=True, circular=True, circular_branch=circular_branch) + + assert np.allclose(dxdt_hat, dxdt_hat_turned) + assert np.allclose(angular_diff(x_hat, x_hat_turned), 0) + + +def test_circular_noise_robustness(): + """Ensure the noisy trajectory from #240, at the high gain the optimizer chose there, still gets the same answer as if the + wrapping were known, across noise realizations""" + course = np.unwrap(fly_course) + for seed in range(20): + x_noisy = course + 0.2*np.random.default_rng(seed).standard_normal(len(course)) + dxdt_oracle = rtsdiff(x_noisy, 0.01, 2, 12, forwardbackward=True)[1] # the same filter, if the wrapping were known + dxdt_hat = rtsdiff(np.angle(np.exp(1j*x_noisy)), 0.01, 2, 12, forwardbackward=True, circular=True)[1] + assert np.allclose(dxdt_hat, dxdt_oracle) + + +def test_circular_argument_errors(): + """Ensure invalid combinations of the circular options are rejected""" + with raises(ValueError): + rtsdiff(np.zeros(5), dt, 1, 2, circular=True, circular_branch="bogus") + with raises(ValueError): + kalman_filter(np.zeros(5), np.zeros(2), np.eye(2), np.eye(2), np.eye(2), np.array([[1., 0]]), np.eye(1), + circular=True, innovation_fn=lambda y, pred: y - pred)