kalman_smooth

This module implements constant-derivative model-based smoothers based on Kalman filtering and its generalization.

pynumdiff.kalman_smooth.rtsdiff(x, dt_or_t, order, log_qr_ratio, forwardbackward=False, axis=0, circular=False)

Perform Rauch-Tung-Striebel smoothing with a naive constant derivative model. Makes use of kalman_filter and rts_smooth, which are made public. constant_X methods in this module call this function.

Parameters:

  • x (np.array[float]) – data series to differentiate. May contain NaN values (missing data); NaNs are excluded from fitting and imputed by dynamical model evolution. May be multidimensional; see axis.

  • dt_or_t (float or array[float]) – This function supports variable step size. This parameter is either the constant \(\Delta t\) if given as a single float, or data locations if given as an array of same length as x.

  • order (int) – which derivative to stabilize in the constant-derivative model

  • log_qr_ratio – the log of the process noise level divided by the measurement noise level, because, per our analysis, the mean result is dependent on the relative rather than absolute size of \(q\) and \(r\).

  • forwardbackward (bool) – indicates whether to run smoother forwards and backwards (usually achieves better estimate at end points)

  • axis (int) – data dimension along which differentiation is performed

  • circular (bool) – if True, treat the measured quantity as a circular variable in radians, wrapping the innovation to \([-\pi, \pi]\). The input x must be in radians; convert degrees with np.deg2rad.

Returns:

  • x_hat (np.array) – estimated (smoothed) x, same shape as input x.

When circular=True, wrapped to \([-\pi, \pi]\). - dxdt_hat (np.array) – estimated derivative of x, same shape as input x

pynumdiff.kalman_smooth.robustdiff(x, dt_or_t, order, log_q, log_r, proc_huberM=6, meas_huberM=0, axis=0)

Perform outlier-robust differentiation by solving the Maximum A Priori optimization problem: \(\text{argmin}_{\{x_n\}} \sum_{n=0}^{N-1} V(R^{-1/2}(y_n - C x_n)) + \sum_{n=1}^{N-1} J(Q_{n-1}^{-1/2}(x_n - A_{n-1} x_{n-1}))\), where \(A,Q,C,R\) come from an assumed constant derivative model and \(V,J\) are the \(\ell_1\) norm or Huber loss rather than the \(\ell_2\) norm optimized by RTS smoothing. This problem is convex, so this method calls convex_smooth, which in turn forms a sparse CVXPY problem and invokes CLARABEL.

Note that for Huber losses, M is the radius where the Huber loss function turns from quadratic to linear. Because all loss function inputs are normalized by noise level, \(q^{1/2}\) or \(r^{1/2}\), M is in units of inlier standard deviation. In other words, this choice affects which portion of inliers might be treated as outliers. For example, assuming Gaussian inliers, the portion beyond \(M\sigma\) is outlier_portion = 2*(1 - scipy.stats.norm.cdf(M)). The inverse of this is M = scipy.stats.norm.ppf(1 - outlier_portion/2). As \(M \to \infty\), Huber becomes the 1/2-sum-of-squares case, \(\frac{1}{2}\|\cdot\|_2^2\), because the normalization constant of the Huber loss (See \(c_2\) in section 6 of this paper, missing a \(\sqrt{\cdot}\) term there, see p2700) approaches 1 as \(M\) increases. Similarly, as M approaches 0, Huber reduces to the \(\ell_1\) norm case, because the normalization constant approaches \(\frac{\sqrt{2}}{M}\), cancelling the \(M\) multiplying \(|\cdot|\) in the Huber function, and leaving behind \(\sqrt{2}\), the proper \(\ell_1\) normalization.

Note that log_q and proc_huberM are coupled, as are log_r and meas_huberM, via the relation \(\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 log_q and log_r be combined into log_qr_ratio as in RTS smoothing without the addition of a new absolute scale parameter, becausee \(q\) and \(r\) interact with the distinct Huber \(M\) parameters.

Parameters:

  • x (np.array[float]) – data series to differentiate. May be multidimensional; see axis.

  • dt_or_t (float or array[float]) – This function supports variable step size. This parameter is either the constant \(\Delta t\) if given as a single float, or data locations if given as an array of same length as x.

  • order (int) – which derivative to stabilize in the constant-derivative model (1=velocity, 2=acceleration, 3=jerk)

  • log_q (float) – base 10 logarithm of process noise variance, so q = 10**log_q

  • log_r (float) – base 10 logarithm of measurement noise variance, so r = 10**log_r

  • proc_huberM (float) – quadratic-to-linear transition point for process loss

  • meas_huberM (float) – quadratic-to-linear transition point for measurement loss

  • axis (int) – data dimension along which differentiation is performed

Returns:

  • x_hat (np.array) – estimated (smoothed) x, same shape as input x

  • dxdt_hat (np.array) – estimated derivative of x, same shape as input x

pynumdiff.kalman_smooth.constant_velocity(x, dt, params=None, options=None, r=None, q=None, forwardbackward=True)

Run a forward-backward constant velocity RTS Kalman smoother to estimate the derivative.

Deprecated, prefer rtsdiff with order 1 instead.

Parameters:

  • x (np.array[float]) – data series to differentiate

  • dt (float) – step size

  • params (list[float]) – (deprecated, prefer r and q)

  • options – (deprecated, prefer forwardbackward) a dictionary consisting of {‘forwardbackward’: (bool)}

  • r (float) – variance of the signal noise

  • q (float) – variance of the constant velocity model

  • forwardbackward (bool) – indicates whether to run smoother forwards and backwards (usually achieves better estimate at end points)

Returns:

  • x_hat (np.array) – estimated (smoothed) x

  • dxdt_hat (np.array) – estimated derivative of x

pynumdiff.kalman_smooth.constant_acceleration(x, dt, params=None, options=None, r=None, q=None, forwardbackward=True)

Run a forward-backward constant acceleration RTS Kalman smoother to estimate the derivative.

Deprecated, prefer rtsdiff with order 2 instead.

Parameters:

  • x (np.array[float]) – data series to differentiate

  • dt (float) – step size

  • params (list[float]) – (deprecated, prefer r and q)

  • options – (deprecated, prefer forwardbackward) a dictionary consisting of {‘forwardbackward’: (bool)}

  • r (float) – variance of the signal noise

  • q (float) – variance of the constant acceleration model

  • forwardbackward (bool) – indicates whether to run smoother forwards and backwards (usually achieves better estimate at end points)

Returns:

  • x_hat (np.array) – estimated (smoothed) x

  • dxdt_hat (np.array) – estimated derivative of x

pynumdiff.kalman_smooth.constant_jerk(x, dt, params=None, options=None, r=None, q=None, forwardbackward=True)

Run a forward-backward constant jerk RTS Kalman smoother to estimate the derivative.

Deprecated, prefer rtsdiff with order 3 instead.

Parameters:

  • x (np.array[float]) – data series to differentiate

  • dt (float) – step size

  • params (list[float]) – (deprecated, prefer r and q)

  • options – (deprecated, prefer forwardbackward) a dictionary consisting of {‘forwardbackward’: (bool)}

  • r (float) – variance of the signal noise

  • q (float) – variance of the constant jerk model

  • forwardbackward (bool) – indicates whether to run smoother forwards and backwards (usually achieves better estimate at end points)

Returns:

  • x_hat (np.array) – estimated (smoothed) x

  • dxdt_hat (np.array) – estimated derivative of x

pynumdiff.kalman_smooth.kalman_filter(y, xhat0, P0, A, Q, C, R, B=None, u=None, save_P=True, innovation_fn=None)

Run the forward pass of a Kalman filter. Expects discrete-time matrices; use scipy.linalg.expm() in the caller to convert from continuous time if needed.

Parameters:

  • y (np.array) – series of measurements, stacked in the direction of axis 0.

  • xhat0 (np.array) – a priori guess of initial state at the time of the first measurement

  • P0 (np.array) – a priori guess of state covariance at the time of first measurement (often identity matrix)

  • A (np.array) – discrete-time state transition matrix. If 2D (\(m \times m\)), the same matrix is used for all steps; if 3D (\(N-1 \times m \times m\)), A[n-1] is used for the transition from step \(n-1\) to \(n\).

  • Q (np.array) – discrete-time process noise covariance. Same 2D or 3D shape convention as A.

  • C (np.array) – measurement matrix

  • R (np.array) – measurement noise covariance

  • B (np.array) – optional discrete-time control matrix, optionally stacked like A.

  • u (np.array) – optional control inputs, stacked in the direction of axis 0

  • save_P (bool) – whether to save history of error covariance and a priori state estimates, used with rts smoothing but nonstandard to compute for ordinary filtering

  • innovation_fn (callable) – optional function taking measurements and predicted measurements and returning the innovation. When None, traditional subtraction is used. This is primarily exposed to handle angles, which have a wrapped domain, so alternative displacement measure lambda y, pred: (y - pred + np.pi) % (2*np.pi) - np.pi is more appropriate.

Returns:

  • 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

  • P_pre (np.array) – a priori estimates of P

  • P_post (np.array) – a posteriori estimates of P

if save_P is True else only xhat_post to save memory

pynumdiff.kalman_smooth.rts_smooth(A, xhat_pre, xhat_post, P_pre, P_post, compute_P_smooth=True)

Perform Rauch-Tung-Striebel smoothing, using information from forward Kalman filtering.

Parameters:

  • A (np.array) – discrete-time state transition matrix. If 2D (\(m \times m\)), the same matrix is used for all steps; if 3D (\(N-1 \times m \times m\)), A[n-1] is used for the transition from step \(n-1\) to \(n\).

  • xhat_pre (np.array) – a priori estimates of xhat from a kalman_filter forward pass

  • xhat_post (np.array) – a posteriori estimates of xhat from a kalman_filter forward pass

  • P_pre (np.array) – a priori estimates of P from a kalman_filter forward pass

  • P_post (np.array) – a posteriori estimates of P from a kalman_filter forward pass

  • compute_P_smooth (bool) – it’s extra work if you don’t need it

Returns:

  • xhat_smooth (np.array) – RTS smoothed xhat

  • P_smooth (np.array) – RTS smoothed P estimates

if compute_P_smooth is True else only xhat_smooth

pynumdiff.kalman_smooth.convex_smooth(y, A, Q, C, R, B=None, u=None, proc_huberM=6, meas_huberM=0)

Solve the optimization problem for robust smoothing using CVXPY.

Parameters:

  • y (np.array[float]) – measurements

  • A (np.array) – discrete-time state transition matrix. If 2D (\(m \times m\)), the same matrix is used for all steps; if 3D (\(N-1 \times m \times m\)), A[n-1] is used for the transition from step \(n-1\) to \(n\).

  • Q (np.array) – discrete-time process noise covariance. Same 2D or 3D shape convention as A.

  • C (np.array) – measurement matrix

  • R (np.array) – measurement noise covariance matrix

  • B (np.array) – optional discrete-time control matrix, optionally stacked like A.

  • u (np.array) – optional control inputs, stacked in the direction of axis 0

  • proc_huberM (float) – Huber loss parameter. \(M=0\) reduces to \(\sqrt{2}\ell_1\).

  • meas_huberM (float) – Huber loss parameter. \(M=\infty\) reduces to \(\frac{1}{2}\ell_2^2\).

Returns:

(np.array) – state estimates (state_dim x N)