work in progress

pavelkomarov · pavelkomarov · commit 5f360a9f8209 · 2025-08-23T00:22:26.000-07:00
diff --git a/docs/source/kalman_smooth.rst b/docs/source/kalman_smooth.rst
@@ -8,4 +8,5 @@ kalman_smooth
 .. autofunction:: pynumdiff.kalman_smooth.constant_velocity
 .. autofunction:: pynumdiff.kalman_smooth.constant_acceleration
 .. autofunction:: pynumdiff.kalman_smooth.constant_jerk
-.. autofunction:: pynumdiff.kalman_smooth.known_dynamics
+.. autofunction:: pynumdiff.kalman_smooth.kalman_filter
+.. autofunction:: pynumdiff.kalman_smooth.rts_smooth
diff --git a/pynumdiff/__init__.py b/pynumdiff/__init__.py
@@ -6,5 +6,5 @@
 from .smooth_finite_difference import meandiff, mediandiff, gaussiandiff, friedrichsdiff, butterdiff
 from .polynomial_fit import splinediff, polydiff, savgoldiff
 from .total_variation_regularization import tvrdiff, velocity, acceleration, jerk, iterative_velocity, smooth_acceleration, jerk_sliding
-from .kalman_smooth import rtsdiff, constant_velocity, constant_acceleration, constant_jerk, known_dynamics
+from .kalman_smooth import kalman_filter, rts_smooth, rtsdiff, constant_velocity, constant_acceleration, constant_jerk
 from .linear_model import spectraldiff, lineardiff, rbfdiff
diff --git a/pynumdiff/kalman_smooth/__init__.py b/pynumdiff/kalman_smooth/__init__.py
@@ -1,3 +1,3 @@
 """This module implements Kalman filters
 """
-from ._kalman_smooth import rtsdiff, constant_velocity, constant_acceleration, constant_jerk, known_dynamics
+from ._kalman_smooth import kalman_filter, rts_smooth, rtsdiff, constant_velocity, constant_acceleration, constant_jerk
diff --git a/pynumdiff/kalman_smooth/_kalman_smooth.py b/pynumdiff/kalman_smooth/_kalman_smooth.py
@@ -1,91 +1,106 @@
 import numpy as np
 from warnings import warn
+from scipy.linalg import expm
 
-####################
-# Helper functions #
-####################
-def _kalman_forward_filter(xhat0, P0, y, A, C, Q, R, u=None, B=None):
-    """Run the forward pass of a Kalman filter
+
+def kalman_filter(y, _t, xhat0, P0, A, Q, C, R, B=None, u=None, save_P=True):
+    """Run the forward pass of a Kalman filter, with regular or irregular step size.
+
+    :param np.array y: series of measurements, stacked along axis 0.
+    :param float or array[float] _t: This function supports variable step size. This parameter is either the constant
+        dt if given as a single float, or data locations if given as an array of same length as :code:`x`.
     :param np.array xhat0: initial estimate of state (often 0) one step before the first measurement
     :param np.array P0: initial guess of state covariance (often identity matrix) before the first measurement
-    :param np.array y: series of measurements, stacked along axis 0. In this case just the raw noisy data (fully observable)
-    :param np.array A: discrete-time state transition matrix
+    :param np.array A: state transition matrix, in discrete time if constant dt, in continuous time if variable dt
+    :param np.array Q: noise covariance matrix, in discrete time if constant dt, in continuous time if variable dt
     :param np.array C: measurement matrix
-    :param np.array Q: discrete-time process noise covariance
     :param np.array R: measurement noise covariance
+    :param np.array B: optional control matrix, in discrete time if constant dt, in continuous time if variable dt
     :param np.array u: optional control input
-    :param np.array B: optional control matrix
+    :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
+
     :return: tuple[np.array, np.array, np.array, np.array] of\n
         - **xhat_pre** -- a priori estimates of xhat, with axis=0 the batch dimension, so xhat[n] gets the nth step
         - **xhat_post** -- a posteriori estimates of xhat
         - **P_pre** -- a priori estimates of P
         - **P_post** -- a posteriori estimates of P
+        if :code:`save_P` is :code:`True` else only **xhat_post** to save memory
     """
-    if B is None: B = np.zeros((A.shape[0], 1))
-    if u is None: u = np.zeros(B.shape[1])
+    # _pre variables are a priori predictions based on only past information
+    # _post variables are a posteriori combinations of all information available at the current step
+    xhat_post = []
+    if save_P: xhat_pre = []; P_pre = []; P_post = []
     xhat = xhat0
     P = P0
 
-    # _pre variables are a priori predictions based on only past information
-    # _post variables are a posteriori combinations of all information available at the current step
-    xhat_pre = []; xhat_post = []; P_pre = []; P_post = []
-    
+    equispaced = isinstance(_t, (float, int)) # I'd rather not call isinstance in the loop
+    control = isinstance(B, np.ndarray) and isinstance(B, np.ndarray) # whether there is a control input
     for n in range(y.shape[0]):
-        xhat_ = A @ xhat + B @ u # ending underscores denote an a priori prediction
-        P_ = A @ P @ A.T + Q
-        xhat_pre.append(xhat_) # the [n]th index holds _{n|n-1} info
-        P_pre.append(P_)
-
-        xhat = xhat_.copy() # copies very important here. See #122
+        if not equispaced:
+            M = expm(np.block([[A, Q],[numpy.zeros(A.shape), -A.T]]) * (_t[n] - _t[n-1])) # form discrete-time matrices TODO doesn't work at n=0
+            An = M[:order+1,:order+1] # upper left block
+            Qn = M[:order+1,order+1:] @ An.T # upper right block
+            if control:
+                M = expm(np.block([[A, B], [np.zeros((A.shape[0], 2*A.shape[1]))]]))
+                Bn = M[:order+1,order+1:] # upper right block 
+        else: # matrices should already be discrete time
+            An, Qn, Bn = A, Q, B
+
+        xhat_ = An @ xhat + Bn @ u if control else An @ xhat # ending underscores denote an a priori prediction
+        P_ = An @ P @ An.T + Qn # the dense matrix multiplies here are the most expensive step
+        xhat = xhat_.copy() # copies, lest modifications to these variables change the a priori estimates. See #122
         P = P_.copy()
         if not np.isnan(y[n]): # handle missing data
             K = P_ @ C.T @ np.linalg.inv(C @ P_ @ C.T + R)
             xhat += K @ (y[n] - C @ xhat_)
             P -= K @ C @ P_
-        xhat_post.append(xhat) # the [n]th index holds _{n|n} info
-        P_post.append(P)
-
-    return np.stack(xhat_pre, axis=0), np.stack(xhat_post, axis=0), np.stack(P_pre, axis=0), np.stack(P_post, axis=0)
-
-
-def _kalman_backward_smooth(A, xhat_pre, xhat_post, P_pre, P_post):
-    """Do the additional Rauch-Tung-Striebel smoothing step
-    :param A: discrete-time state transition matrix
-    :param xhat_pre: a priori estimates of xhat
-    :param xhat_post: a posteriori estimates of xhat
-    :param P_pre: a priori estimates of P
-    :param P_post: a posteriori estimates of P
+        # the [n]th index of pre variables holds _{n|n-1} info; the [n]th index of post variables holds _{n|n} info
+        xhat_post.append(xhat)
+        if save_P: xhat_pre.append(xhat_); P_pre.append(P_); P_post.append(P)
+
+    xhat_post = np.stack(xhat_post, axis=0)
+    return xhat_post if not save_P else (np.stack(xhat_pre, axis=0), xhat_post,
+                                         np.stack(P_pre, axis=0), np.stack(P_post, axis=0))
+
+
+def rts_smooth(_t, A, xhat_pre, xhat_post, P_pre, P_post, compute_P_smooth=False):
+    """Perform Rauch-Tung-Striebel smoothing, using information from forward Kalman filtering.
+
+    :param float or array[float] _t: This function supports variable step size. This parameter is either the constant
+        dt if given as a single float, or data locations if given as an array of same length as :code:`x`.
+    :param np.array A: state transition matrix, in discrete time if constant dt, in continuous time if variable dt
+    :param list[np.array] xhat_pre: a priori estimates of xhat from a kalman_filter forward pass
+    :param list[np.array] xhat_post: a posteriori estimates of xhat from a kalman_filter forward pass
+    :param list[np.array] P_pre: a priori estimates of P from a kalman_filter forward pass
+    :param list[np.array] P_post: a posteriori estimates of P from a kalman_filter forward pass
+    :param bool compute_P_smooth: it's extra work if you don't need it
+    
     :return: tuple[np.array, np.array] of\n
         - **xhat_smooth** -- RTS smoothed xhat
         - **P_smooth** -- RTS smoothed P
+        if :code:`compute_P_smooth` is :code:`True` else only **xhat_smooth**
     """
-    xhat_smooth = [xhat_post[-1]]
-    P_smooth = [P_post[-1]]
+    xhat_smooth = np.empty(xhat_post.shape); xhat_smooth[-1] = xhat_post[-1] # preallocate arrays
+    if compute_P_smooth: P_smooth = np.empty(P_post.shape); P_smooth[-1] = P_post[-1]
+    
+    equispaced = isinstance(_t, (int, float)) # I'd rather not call isinstance in a loop when it's avoidable
     for n in range(xhat_pre.shape[0]-2, -1, -1):
-        C_RTS = P_post[n] @ A.T @ np.linalg.inv(P_pre[n+1]) # the [n+1]th index holds _{n+1|n} info 
-        xhat_smooth.append(xhat_post[n] + C_RTS @ (xhat_smooth[-1] - xhat_pre[n+1])) # The original authors use C, not to
-        P_smooth.append(P_post[n] + C_RTS @ (P_smooth[-1] - P_pre[n+1]) @ C_RTS.T) # be confused with the measurement matrix
+        An = A if equispaced else expm(A * (_t[n+1] - _t[n])) # state transition from n to n+1, per the paper
+        C_RTS = P_post[n] @ An.T @ np.linalg.inv(P_pre[n+1]) # the [n+1]th index holds _{n+1|n} info 
+        xhat_smooth[n] = xhat_post[n] + C_RTS @ (xhat_smooth[n+1] - xhat_pre[n+1]) # The original authors use C, not to be confused
+        if compute_P_smooth: P_smooth[n] = P_post[n] + C_RTS @ (P_smooth[n+1] - P_pre[n+1]) @ C_RTS.T # with the measurement matrix
 
-    return np.stack(xhat_smooth[::-1], axis=0), np.stack(P_smooth[::-1], axis=0) # reverse lists
+    return xhat_smooth if not compute_P_smooth else (xhat_smooth, P_smooth)
 
 
-def _RTS_smooth(xhat0, P0, y, A, C, Q, R, u=None, B=None):
-    """forward-backward Kalman/Rauch-Tung-Striebel smoother. For params see the helper functions.
-    """
-    xhat_pre, xhat_post, P_pre, P_post = _kalman_forward_filter(xhat0, P0, y, A, C, Q, R) # noisy x are the "y" in Kalman-land
-    xhat_smooth, _ = _kalman_backward_smooth(A, xhat_pre, xhat_post, P_pre, P_post) # not doing anything with P_smooth
-    return xhat_smooth
-
-
-#########################################
-# Constant 1st, 2nd, and 3rd derivative #
-#########################################
-def rtsdiff(x, dt, order, qr_ratio, forwardbackward):
+def rtsdiff(x, _t, order, qr_ratio, forwardbackward):
     """Perform Rauch-Tung-Striebel smoothing with a naive constant derivative model. Other constant derivative
     methods in this module call this function.
 
     :param np.array[float] x: data series to differentiate
-    :param float dt: step size
+    :param float or array[float] _t: This function supports variable step size. This parameter is either the constant
+        dt if given as a single float, or data locations if given as an array of same length as :code:`x`.
     :param int order: which derivative to stabilize in the constant-derivative model
     :param qr_ratio: the process noise level of the divided by the measurement noise level, because,
         per `our analysis <https://github.com/florisvb/PyNumDiff/issues/139>`_, the mean result is
@@ -97,49 +112,49 @@ def rtsdiff(x, dt, order, qr_ratio, forwardbackward):
         - **x_hat** -- estimated (smoothed) x
         - **dxdt_hat** -- estimated derivative of x
     """
+    if isinstance(_t, (np.ndarray, list)) and len(x) != len(_t):
+        raise ValueError("If `_t` is given as array-like, must have same length as `x`.")
+
     q = 10**int(np.log10(qr_ratio)/2) # even-ish split of the powers across 0
     r = q/qr_ratio
 
-    if order == 1:
-        A = np.array([[1, dt], [0, 1]]) # states are x, x'. This basically does an Euler update.
-        C = np.array([[1, 0]]) # we measure only y = noisy x
-        R = np.array([[r]])
-        Q = np.array([[1e-16, 0], [0, q]]) # uncertainty is around the velocity
-        P0 = np.array(100*np.eye(2)) # See #110 for why this choice of P0
-    elif order == 2:
-        A = np.array([[1, dt, (dt**2)/2], # states are x, x', x"
-                      [0, 1, dt],
-                      [0, 0,  1]])
-        C = np.array([[1, 0, 0]]) # we measure only y = noisy x
-        R = np.array([[r]])
-        Q = np.array([[1e-16, 0, 0],
-                      [0, 1e-16, 0],
-                      [0,     0, q]]) # uncertainty is around the acceleration
-        P0 = np.array(100*np.eye(3)) # See #110 for why this choice of P0
-    elif order == 3:
-        A = np.array([[1, dt, (dt**2)/2, (dt**3)/6], # states are x, x', x", x'"
-                      [0, 1, dt, (dt**2)/2],
-                      [0, 0,  1, dt],
-                      [0, 0,  0, 1]])
-        C = np.array([[1, 0, 0, 0]]) # we measure only y = noisy x
-        R = np.array([[r]])
-        Q = np.array([[1e-16,  0, 0,     0],
-                      [0, 1e-16, 0,     0],
-                      [0,     0, 1e-16, 0],
-                      [0,     0, 0,     q]]) # uncertainty is around the jerk
-        P0 = np.array(100*np.eye(4)) # See #110 for why this choice of P0
-
+    A = np.diag(np.ones(order), 1) # continuous-time A just has 1s on the first diagonal (where 0th is main diagonal)
+    Q = np.zeros(A.shape); Q[-1,-1] = q # continuous-time uncertainty around the last derivative
+    C = np.zeros((1, order+1)); C[0,0] = 1 # we measure only y = noisy x
+    R = np.array([[r]]) # 1 observed state, so this is 1x1
+    P0 = 100*np.eye(order+1) # See #110 for why this choice of P0
     xhat0 = np.zeros(A.shape[0]); xhat0[0] = x[0] # The first estimate is the first seen state. See #110
-    xhat_smooth = _RTS_smooth(xhat0, P0, x, A, C, Q, R) # noisy x are the "y" in Kalman-land  
+
+    if isinstance(_t, (float, int)):
+        M = expm(np.block([[A, Q],[numpy.zeros(A.shape), -A.T]]) * _t) # form discrete-time versions
+        A = M[:order+1,:order+1]
+        Q = M[:order+1,order+1:] @ A.T
+
+    xhat_pre, xhat_post, P_pre, P_post = kalman_filter(x, _t, xhat0, P0, A, Q, C, R) # noisy x are the "y" in Kalman-land
+    xhat_smooth = rts_smooth(_t, A, xhat_pre, xhat_post, P_pre, P_post) # not doing anything with P_smooth  
     x_hat_forward = xhat_smooth[:, 0] # first dimension is time, so slice first element at all times
     dxdt_hat_forward = xhat_smooth[:, 1]
 
     if not forwardbackward: # bounce out here if not doing the same in reverse and then combining
         return x_hat_forward, dxdt_hat_forward
 
+    # for backward case, if discrete time invert the matrix, and for continuous time reverse _t, because then dts will go negative
     xhat0[0] = x[-1] # starting from the other end of the signal
-    Ainv = np.linalg.inv(A) # dynamics are inverted
-    xhat_smooth = _RTS_smooth(xhat0, P0, x[::-1], Ainv, C, Q, R) # noisy x are the "y" in Kalman-land    
+
+    ### TODO fix between here and next hashes
+    if isinstance(_t, (float, int)):
+        M = expm(np.block([[A, Q],[numpy.zeros(A.shape), -A.T]]) * -_t) # form discrete-time versions
+        A = M[:order+1,:order+1] # inverse of A
+        Q = M[:order+1,order+1:] @ A.T # 
+
+    Ainv = np.linalg.inv(A_c) # dynamics are inverted, same as expm() with negative dt
+    _t_rev = _t[::-1]  _t
+    
+    xhat_pre, xhat_post, P_pre, P_post = _kalman_forward_filter(x[::-1], _t[::-1], xhat0, P0, A, Q, C, R) if \
+        isinstance(_t, (np.ndarray, list)) else _kalman_forward_filter(x[::-1], _t, xhat0, P0, np.linalg.inv(A), Q, C, R)
+    xhat_smooth = _kalman_backward_smooth(_t_rev, A_c, xhat_pre, xhat_post, P_pre, P_post)
+    ###
+
     x_hat_backward = xhat_smooth[:, 0][::-1] # the result is backwards still, so reverse it
     dxdt_hat_backward = xhat_smooth[:, 1][::-1]
 
@@ -235,41 +250,3 @@ def constant_jerk(x, dt, params=None, options=None, r=None, q=None, forwardbackw
         raise ValueError("`q` and `r` must be given.")
 
     return rtsdiff(x, dt, 3, q/r, forwardbackward)
-
-
-def known_dynamics(x, params, u=None, options=None, xhat0=None, P0=None, A=None,
-    B=0, C=None, Q=None, R=None, smooth=True):
-    """Run a forward RTS Kalman smoother given known dynamics.
-
-    :param np.array[float] x: data series of noisy measurements
-    :param list params: (**deprecated**, prefer :code:`xhat0`, :code:`P0`, :code:`A`,
-        :code:`B`, :code:`C`, :code:`R`, and :code:`Q`), a list in the order here (note flip of Q and R)
-    :param np.array[float] u: series of control inputs
-    :param options: (**deprecated**, prefer :code:`smooth`)
-        a dictionary consisting of {'smooth': (bool)}
-    :param np.array xhat0: inital condition, length N = number of states
-    :param np.array P0: initial covariance matrix of NxN
-    :param np.array A: dynamics matrix, NxN
-    :param np.array B: control input matrix, NxM, M = number of measurements
-    :param np.array C: measurement dynamics, MxN
-    :param np.array Q: covariance matrix for the model, NxN
-    :param np.array R: covariance matrix for the measurements, MxM
-    :param bool smooth: whether to run the RTS smoother step
-
-    :return: np.array **x_hat** -- estimated (smoothed) x
-    """ # Why not also returning derivative here?
-    if params != None:
-        warn("`params` and `options` parameters will be removed in a future version. Use `xhat0`, " +
-            "`P0`, `A`, `B`, `C`, `Q`, `R`, and `smooth` instead.", DeprecationWarning)
-        xhat0, P0, A, B, C, R, Q = params
-        if options != None:
-            if 'smooth' in options: smooth = options['smooth']
-    elif None in [xhat0, P0, A, C, R, Q]:
-        raise ValueError("`xhat0`, `P0`, `A`, `C`, `Q`, and `R` must be given.")
-
-    xhat_pre, xhat_post, P_pre, P_post = _kalman_forward_filter(xhat0, P0, x, A, C, Q, R, u, B) # noisy x are the "y" in Kalman-land
-    if not smooth:
-        return xhat_post
-
-    xhat_smooth, _ = _kalman_backward_smooth(A, xhat_pre, xhat_post, P_pre, P_post)
-    return xhat_smooth # We're not calculating a derivative here. Why not?
diff --git a/pynumdiff/linear_model/_linear_model.py b/pynumdiff/linear_model/_linear_model.py
@@ -238,7 +238,7 @@ def rbfdiff(x, _t, sigma=1, lmbd=0.01):
 
     :param np.array[float] x: data to differentiate
     :param float or array[float] _t: This function supports variable step size. This parameter is either the constant
-        step size if given as a single float, or data locations if given as an array of same length as :code:`x`.
+        dt if given as a single float, or data locations if given as an array of same length as :code:`x`.
     :param float sigma: controls width of radial basis function
     :param float lmbd: controls strength of bias toward data
 
diff --git a/pynumdiff/polynomial_fit/_polynomial_fit.py b/pynumdiff/polynomial_fit/_polynomial_fit.py
@@ -11,7 +11,7 @@ def splinediff(x, _t, params=None, options={}, degree=3, s=None, num_iterations=
 
     :param np.array[float] x: data to differentiate
     :param float or array[float] _t: This function supports variable step size. This parameter is either the constant
-        step size if given as a single float, or data locations if given as an array of same length as :code:`x`.
+        dt if given as a single float, or data locations if given as an array of same length as :code:`x`.
     :param list params: (**deprecated**, prefer :code:`degree`, :code:`cutoff_freq`, and :code:`num_iterations`)
     :param dict options: (**deprecated**, prefer :code:`num_iterations`) an empty dictionary or {'iterate': (bool)}
     :param int degree: polynomial degree of the spline. A kth degree spline can be differentiated k times.
