Observers

Observers estimate the internal state \(\hat{x}_k\) from plant observations \(y_k\) and control inputs \(u_k\). h always returns \(\hat{x}_k\) so the output can be fed directly into a controller block.

Kalman Filter (KF)

Optimal estimator for linear, Gaussian systems. See the kalman_filter notebook for derivation, API reference, and examples.

import numpy as np
from typing import Callable
from jaxtyping import Float, jaxtyped
from beartype import beartype


@jaxtyped(typechecker=beartype)
def kf_f(
    zk: tuple[Float[np.ndarray, "n"], Float[np.ndarray, "n n"]],
    uk: float,
    yk: Float[np.ndarray, "m"],
    Fk: Float[np.ndarray, "n n"],
    Bk: Float[np.ndarray, "n"],
    Hk: Float[np.ndarray, "m n"],
    Qk: Float[np.ndarray, "n n"],
    Rk: Float[np.ndarray, "m m"],
) -> tuple[Float[np.ndarray, "n"], Float[np.ndarray, "n n"]]:
    """Kalman filter predict+update; returns updated Kalman filter state.

    Args:
        zk: KF state — 2-tuple of (x̂_k, P_k), state estimate and error covariance.
        uk: Scalar control input.
        yk: Measurement vector (m,).
        Fk: State transition matrix (n, n) — maps x_k → x_{k+1}.
        Bk: Control input matrix (n,) — maps scalar u_k into state space.
        Hk: Observation matrix (m, n) — maps state to measurement space.
        Qk: Process noise covariance (n, n) — encodes model uncertainty.
        Rk: Measurement noise covariance (m, m) — encodes sensor noise.

    Returns:
        tuple — (x̂_{k+1}, P_{k+1}), the updated state estimate and error covariance.
        Fed back as zk on the next step; not published directly to ROS.
    """
    xk_hat, Pk = zk
    x_pred = Fk @ xk_hat + Bk * uk
    P_pred = Fk @ Pk @ Fk.T + Qk
    S = Hk @ P_pred @ Hk.T + Rk
    K = P_pred @ Hk.T @ np.linalg.inv(S)
    yk = np.atleast_1d(yk)
    x_upd = x_pred + K @ (yk - Hk @ x_pred)
    P_upd = (np.eye(len(xk_hat)) - K @ Hk) @ P_pred
    return (x_upd, P_upd)


@jaxtyped(typechecker=beartype)
def kf_h(
    zk: tuple[Float[np.ndarray, "n"], Float[np.ndarray, "n n"]],
) -> Float[np.ndarray, "n"]:
    """Return the current state estimate from the KF state tuple.

    Args:
        zk: KF state — 2-tuple of (x̂_k, P_k), state estimate and error covariance.

    Returns:
        ndarray(n,) — state estimate x̂_k.
        Published as Float64MultiArray via py2ros_float64_multiarray.
    """
    return zk[0]

Extended Kalman Filter (EKF)

Extends the KF to nonlinear systems by linearising around the current estimate\nusing user-supplied Jacobians \(F^J_k = \\partial f / \\partial x\) and \(H^J_k = \\partial h / \\partial x\).\nSee the ekf notebook for derivation, API reference, and examples.

import numpy as np
from typing import Callable
from jaxtyping import Float, jaxtyped
from beartype import beartype


@jaxtyped(typechecker=beartype)
def ekf_f(
    zk: tuple[Float[np.ndarray, "n"], Float[np.ndarray, "n n"]],
    uk: float,
    yk: Float[np.ndarray, "m"],
    f_nl: Callable,
    h_nl: Callable,
    Fjk: Float[np.ndarray, "n n"],
    Hjk: Float[np.ndarray, "m n"],
    Qk: Float[np.ndarray, "n n"],
    Rk: Float[np.ndarray, "m m"],
) -> tuple[Float[np.ndarray, "n"], Float[np.ndarray, "n n"]]:
    """EKF predict+update via Jacobian linearisation; returns updated state and covariance.

    Args:
        zk:   EKF state — 2-tuple of (x̂_k, P_k), state estimate and error covariance.
        uk:   Scalar control input.
        yk:   Measurement vector (m,).
        f_nl: Nonlinear state transition f(x, u) → x_next.
        h_nl: Nonlinear observation function h(x) → y.
        Fjk:  Jacobian ∂f/∂x evaluated at x̂_{k-1} (n, n).
        Hjk:  Jacobian ∂h/∂x evaluated at x̂_{k|k-1} (m, n).
        Qk:   Process noise covariance (n, n).
        Rk:   Measurement noise covariance (m, m).

    Returns:
        tuple — (x̂_{k+1}, P_{k+1}), the updated state estimate and error covariance.
        Fed back as zk on the next step; not published directly to ROS.
    """
    x, P = zk
    x_pred = f_nl(x, uk)
    P_pred = Fjk @ P @ Fjk.T + Qk
    S = Hjk @ P_pred @ Hjk.T + Rk
    K = P_pred @ Hjk.T @ np.linalg.inv(S)
    yk = np.atleast_1d(yk)
    x_upd = x_pred + K @ (yk - h_nl(x_pred))
    P_upd = (np.eye(len(x)) - K @ Hjk) @ P_pred
    return (x_upd, P_upd)


@jaxtyped(typechecker=beartype)
def ekf_h(
    zk: tuple[Float[np.ndarray, "n"], Float[np.ndarray, "n n"]],
) -> Float[np.ndarray, "n"]:
    """Return the current state estimate from the EKF state tuple.

    Args:
        zk: EKF state — 2-tuple of (x̂_k, P_k), state estimate and error covariance.

    Returns:
        ndarray(n,) — state estimate x̂_k.
        Published as Float64MultiArray via py2ros_float64_multiarray.
    """
    return zk[0]

Unscented Kalman Filter (UKF)

Handles nonlinear systems without Jacobians by propagating a set of deterministically chosen sigma points through the true nonlinear functions.See the ukf notebook for derivation, API reference, and examples.

@jaxtyped(typechecker=beartype)
def ukf_f(
    zk: tuple[Float[np.ndarray, "n"], Float[np.ndarray, "n n"]],
    uk: float,
    yk: Float[np.ndarray, "m"],
    f_nl: Callable,
    h_nl: Callable,
    Qk: Float[np.ndarray, "n n"],
    Rk: Float[np.ndarray, "m m"],
    alpha: float = 1e-3,
    beta: float = 2.0,
    kappa: float = 0.0,
) -> tuple[Float[np.ndarray, "n"], Float[np.ndarray, "n n"]]:
    """UKF predict+update via sigma-point propagation; returns updated state and covariance.

    Args:
        zk:    UKF state — 2-tuple of (x̂_k, P_k), state estimate and error covariance.
        uk:    Scalar control input.
        yk:    Measurement vector (m,).
        f_nl:  Nonlinear state transition f(x, u) → x_next.
        h_nl:  Nonlinear observation function h(x) → y.
        Qk:    Process noise covariance (n, n).
        Rk:    Measurement noise covariance (m, m).
        alpha: Sigma point spread around the mean (default 1e-3).
        beta:  Distribution parameter; 2.0 optimal for Gaussian (default 2.0).
        kappa: Secondary scaling parameter, typically 0 or 3-n (default 0.0).

    Returns:
        tuple — (x̂_{k+1}, P_{k+1}), the updated state estimate and error covariance.
        Fed back as zk on the next step; not published directly to ROS.
    """
    x, P = zk
    n = len(x)
    lam = alpha**2 * (n + kappa) - n

    sqrtP = np.linalg.cholesky((n + lam) * P)
    sigmas = np.column_stack(
        [x] + [x + sqrtP[:, i] for i in range(n)] + [x - sqrtP[:, i] for i in range(n)]
    )
    Wm = np.full(2 * n + 1, 1.0 / (2 * (n + lam)))
    Wm[0] = lam / (n + lam)
    Wc = Wm.copy()
    Wc[0] += 1 - alpha**2 + beta

    # Predict
    sf = np.column_stack([f_nl(sigmas[:, i], uk) for i in range(2 * n + 1)])
    xp = sf @ Wm
    Pp = (
        sum(Wc[i] * np.outer(sf[:, i] - xp, sf[:, i] - xp) for i in range(2 * n + 1))
        + Qk
    )

    # Update
    yk = np.atleast_1d(yk)
    sh = np.column_stack([np.atleast_1d(h_nl(sf[:, i])) for i in range(2 * n + 1)])
    yp = sh @ Wm
    S = (
        sum(Wc[i] * np.outer(sh[:, i] - yp, sh[:, i] - yp) for i in range(2 * n + 1))
        + Rk
    )
    Pxy = sum(Wc[i] * np.outer(sf[:, i] - xp, sh[:, i] - yp) for i in range(2 * n + 1))
    K = Pxy @ np.linalg.inv(S)
    return (xp + K @ (yk - yp), Pp - K @ S @ K.T)


@jaxtyped(typechecker=beartype)
def ukf_h(
    zk: tuple[Float[np.ndarray, "n"], Float[np.ndarray, "n n"]],
) -> Float[np.ndarray, "n"]:
    """Return the current state estimate from the UKF state tuple.

    Args:
        zk: UKF state — 2-tuple of (x̂_k, P_k), state estimate and error covariance.

    Returns:
        ndarray(n,) — state estimate x̂_k.
        Published as Float64MultiArray via py2ros_float64_multiarray.
    """
    return zk[0]