Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
36 changes: 28 additions & 8 deletions pynumdiff/kalman_smooth.py
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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 <https://github.com/florisvb/PyNumDiff/issues/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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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.

Expand All @@ -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 <https://github.com/florisvb/PyNumDiff/issues/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 "
Expand Down Expand Up @@ -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
Expand All @@ -160,15 +180,15 @@ 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]

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
Expand Down
95 changes: 90 additions & 5 deletions pynumdiff/tests/test_diff_methods.py
Original file line number Diff line number Diff line change
@@ -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)
Expand Down Expand Up @@ -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))
Expand All @@ -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)
Loading