nrl-tracker 1.9.1__py3-none-any.whl → 1.9.2__py3-none-any.whl

pytcl/dynamic_estimation/imm.py

@@ -468,6 +468,35 @@ def imm_predict_update(
     -------
     result : IMMUpdate
         Updated states, covariances, and mode probabilities.
+
+    Examples
+    --------
+    Track a target with 2 motion modes (CV and CA):
+
+    >>> import numpy as np
+    >>> # Two modes: constant velocity and constant acceleration
+    >>> states = [np.array([0, 1, 0, 1]), np.array([0, 1, 0, 1])]
+    >>> covs = [np.eye(4) * 0.1, np.eye(4) * 0.1]
+    >>> probs = np.array([0.9, 0.1])  # likely CV mode
+    >>> # Mode transition matrix (90% stay, 10% switch)
+    >>> trans = np.array([[0.9, 0.1], [0.1, 0.9]])
+    >>> # Dynamics and measurement matrices for each mode
+    >>> F_cv = np.array([[1, 1, 0, 0], [0, 1, 0, 0], [0, 0, 1, 1], [0, 0, 0, 1]])
+    >>> F_ca = F_cv.copy()  # simplified
+    >>> H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]])
+    >>> Q = np.eye(4) * 0.01
+    >>> R = np.eye(2) * 0.1
+    >>> z = np.array([1.0, 1.0])
+    >>> result = imm_predict_update(states, covs, probs, trans, z,
+    ...                             [F_cv, F_ca], [Q, Q], [H, H], [R, R])
+    >>> len(result.mode_probs)
+    2
+
+    See Also
+    --------
+    imm_predict : IMM prediction step only.
+    imm_update : IMM update step only.
+    IMMEstimator : Object-oriented interface.
     """
     pred = imm_predict(
         mode_states, mode_covs, mode_probs, transition_matrix, F_list, Q_list

pytcl/dynamic_estimation/information_filter.py

@@ -119,6 +119,20 @@ def information_to_state(
         State estimate.
     P : ndarray
         State covariance.
+
+    Examples
+    --------
+    >>> import numpy as np
+    >>> # Information form: Y = inv(P), y = Y @ x
+    >>> x_true = np.array([1.0, 2.0])
+    >>> P_true = np.eye(2) * 0.5
+    >>> Y = np.linalg.inv(P_true)  # Information matrix
+    >>> y = Y @ x_true              # Information vector
+    >>> x_recovered, P_recovered = information_to_state(y, Y)
+    >>> np.allclose(x_recovered, x_true)
+    True
+    >>> np.allclose(P_recovered, P_true)
+    True
     """
     y = np.asarray(y, dtype=np.float64).flatten()
     Y = np.asarray(Y, dtype=np.float64)
@@ -149,6 +163,18 @@ def state_to_information(
         Information vector.
     Y : ndarray
         Information matrix.
+
+    Examples
+    --------
+    >>> import numpy as np
+    >>> x = np.array([1.0, 2.0])
+    >>> P = np.eye(2) * 0.5  # Covariance
+    >>> y, Y = state_to_information(x, P)
+    >>> Y  # Should be inv(P) = 2*I
+    array([[2., 0.],
+           [0., 2.]])
+    >>> y  # Should be Y @ x = [2, 4]
+    array([2., 4.])
     """
     x = np.asarray(x, dtype=np.float64).flatten()
     P = np.asarray(P, dtype=np.float64)
@@ -323,6 +349,24 @@ def srif_predict(
     R_pred : ndarray
         Predicted square root information matrix.
 
+    Examples
+    --------
+    >>> import numpy as np
+    >>> n = 2
+    >>> # Initialize with known state
+    >>> P0 = np.eye(n)
+    >>> R0 = np.linalg.inv(np.linalg.cholesky(P0)).T
+    >>> x0 = np.array([1.0, 0.5])
+    >>> r0 = R0 @ x0
+    >>> # Prediction step
+    >>> F = np.array([[1, 1], [0, 1]])
+    >>> Q = np.eye(2) * 0.1
+    >>> r_pred, R_pred = srif_predict(r0, R0, F, Q)
+    >>> r_pred.shape
+    (2,)
+    >>> R_pred.shape
+    (2, 2)
+
     Notes
     -----
     The SRIF prediction uses:
@@ -400,6 +444,26 @@ def srif_update(
     R_upd : ndarray
         Updated square root information matrix.
 
+    Examples
+    --------
+    >>> import numpy as np
+    >>> n = 2
+    >>> # Prior in SRIF form
+    >>> P_prior = np.eye(n) * 2.0
+    >>> R_prior = np.linalg.inv(np.linalg.cholesky(P_prior)).T
+    >>> x_prior = np.array([1.0, 0.5])
+    >>> r_prior = R_prior @ x_prior
+    >>> # Measurement update
+    >>> z = np.array([1.2])  # Position measurement
+    >>> H = np.array([[1, 0]])
+    >>> R_meas = np.array([[0.5]])  # Measurement noise covariance
+    >>> r_upd, R_upd = srif_update(r_prior, R_prior, z, H, R_meas)
+    >>> r_upd.shape
+    (2,)
+    >>> # Updated information matrix has more information (higher values)
+    >>> np.linalg.det(R_upd.T @ R_upd) > np.linalg.det(R_prior.T @ R_prior)
+    True
+
     Notes
     -----
     The update uses QR decomposition to combine the prior information

pytcl/dynamic_estimation/kalman/extended.py

@@ -263,6 +263,23 @@ def ekf_predict_auto(
     -------
     result : KalmanPrediction
         Predicted state and covariance.
+
+    Examples
+    --------
+    >>> import numpy as np
+    >>> # Simple nonlinear dynamics
+    >>> def f(x):
+    ...     return np.array([x[0] + x[1], 0.9 * x[1]])
+    >>> x = np.array([0.0, 1.0])
+    >>> P = np.eye(2) * 0.1
+    >>> Q = np.eye(2) * 0.01
+    >>> pred = ekf_predict_auto(x, P, f, Q)
+    >>> pred.x
+    array([1., 0.9])
+
+    See Also
+    --------
+    ekf_predict : EKF prediction with explicit Jacobian.
     """
     x = np.asarray(x, dtype=np.float64).flatten()
     F = numerical_jacobian(f, x, dx)
@@ -299,6 +316,24 @@ def ekf_update_auto(
     -------
     result : KalmanUpdate
         Updated state and covariance.
+
+    Examples
+    --------
+    >>> import numpy as np
+    >>> # Range measurement h(x) = sqrt(x[0]^2 + x[1]^2)
+    >>> def h(x):
+    ...     return np.array([np.sqrt(x[0]**2 + x[1]**2)])
+    >>> x = np.array([3.0, 4.0])  # predicted position
+    >>> P = np.eye(2) * 0.5
+    >>> z = np.array([5.1])  # measured range
+    >>> R = np.array([[0.1]])
+    >>> upd = ekf_update_auto(x, P, z, h, R)
+    >>> np.linalg.norm(upd.x)  # estimate moves toward measurement
+    5.0...
+
+    See Also
+    --------
+    ekf_update : EKF update with explicit Jacobian.
     """
     x = np.asarray(x, dtype=np.float64).flatten()
     H = numerical_jacobian(h, x, dx)
@@ -344,6 +379,27 @@ def iterated_ekf_update(
     -------
     result : KalmanUpdate
         Updated state and covariance after convergence.
+
+    Examples
+    --------
+    >>> import numpy as np
+    >>> # Bearing-only measurement (highly nonlinear)
+    >>> def h(x):
+    ...     return np.array([np.arctan2(x[1], x[0])])
+    >>> def H_func(x):
+    ...     r2 = x[0]**2 + x[1]**2
+    ...     return np.array([[-x[1]/r2, x[0]/r2]])
+    >>> x = np.array([10.0, 10.0])
+    >>> P = np.eye(2) * 5.0
+    >>> z = np.array([np.radians(50)])  # measured bearing
+    >>> R = np.array([[0.01]])
+    >>> upd = iterated_ekf_update(x, P, z, h, H_func, R)
+    >>> upd.x.shape
+    (2,)
+
+    See Also
+    --------
+    ekf_update : Standard (non-iterated) EKF update.
     """
     x = np.asarray(x, dtype=np.float64).flatten()
     P = np.asarray(P, dtype=np.float64)

pytcl/dynamic_estimation/kalman/linear.py

@@ -282,6 +282,22 @@ def kf_predict_update(
     result : KalmanUpdate
         Updated state and covariance with innovation statistics.
 
+    Examples
+    --------
+    Track a 1D constant-velocity target with position measurements:
+
+    >>> import numpy as np
+    >>> x = np.array([0.0, 1.0])  # [position, velocity]
+    >>> P = np.eye(2) * 0.1
+    >>> F = np.array([[1, 1], [0, 1]])  # CV model, T=1s
+    >>> Q = np.array([[0.25, 0.5], [0.5, 1.0]])  # process noise
+    >>> H = np.array([[1, 0]])  # measure position only
+    >>> R = np.array([[0.5]])  # measurement noise variance
+    >>> z = np.array([1.1])  # measured position
+    >>> result = kf_predict_update(x, P, z, F, Q, H, R)
+    >>> result.x  # updated state
+    array([1.0..., 1.0...])
+
     See Also
     --------
     kf_predict : Prediction step only.
@@ -336,6 +352,30 @@ def kf_smooth(
         G = P_filt @ F' @ P_pred^{-1}
         x_smooth = x_filt + G @ (x_smooth_next - x_pred)
         P_smooth = P_filt + G @ (P_smooth_next - P_pred) @ G'
+
+    Examples
+    --------
+    Apply RTS smoothing to improve a filtered estimate:
+
+    >>> import numpy as np
+    >>> # Filtered estimate at time k
+    >>> x_filt = np.array([1.0, 0.9])
+    >>> P_filt = np.array([[0.1, 0.05], [0.05, 0.2]])
+    >>> # Predicted estimate at time k+1 (from forward pass)
+    >>> x_pred = np.array([1.9, 0.9])
+    >>> P_pred = np.array([[0.35, 0.25], [0.25, 0.4]])
+    >>> # Smoothed estimate at time k+1 (already computed)
+    >>> x_smooth_next = np.array([2.0, 1.0])
+    >>> P_smooth_next = np.array([[0.08, 0.03], [0.03, 0.15]])
+    >>> F = np.array([[1, 1], [0, 1]])  # CV model
+    >>> x_s, P_s = kf_smooth(x_filt, P_filt, x_pred, P_pred,
+    ...                      x_smooth_next, P_smooth_next, F)
+    >>> x_s  # smoothed state at time k
+    array([1.0..., 0.9...])
+
+    See Also
+    --------
+    kf_predict_update : Forward filtering step.
     """
     x_filt = np.asarray(x_filt, dtype=np.float64).flatten()
     P_filt = np.asarray(P_filt, dtype=np.float64)
@@ -390,6 +430,21 @@ def information_filter_predict(
     Y_pred : ndarray
         Predicted information matrix.
 
+    Examples
+    --------
+    >>> import numpy as np
+    >>> # Convert from state form to information form
+    >>> x = np.array([1.0, 0.5])
+    >>> P = np.eye(2) * 0.1
+    >>> Y = np.linalg.inv(P)
+    >>> y = Y @ x
+    >>> # Predict with constant velocity model
+    >>> F = np.array([[1, 1], [0, 1]])
+    >>> Q = np.eye(2) * 0.01
+    >>> y_pred, Y_pred = information_filter_predict(y, Y, F, Q)
+    >>> y_pred.shape
+    (2,)
+
     Notes
     -----
     The prediction requires converting to state space, predicting,
@@ -445,6 +500,20 @@ def information_filter_update(
     Y_upd : ndarray
         Updated information matrix.
 
+    Examples
+    --------
+    >>> import numpy as np
+    >>> # Initial information form (from prior)
+    >>> Y = np.eye(2) * 10.0  # High prior information
+    >>> y = np.array([10.0, 5.0])  # Corresponds to state [1.0, 0.5]
+    >>> # Measurement z = H @ x + noise
+    >>> z = np.array([1.1])
+    >>> H = np.array([[1, 0]])  # Measure position only
+    >>> R = np.array([[0.1]])
+    >>> y_upd, Y_upd = information_filter_update(y, Y, z, H, R)
+    >>> Y_upd[0, 0] > Y[0, 0]  # Information increased
+    True
+
     Notes
     -----
     The update in information form is additive:

pytcl/dynamic_estimation/kalman/unscented.py

@@ -138,6 +138,17 @@ def sigma_points_julier(
     result : SigmaPoints
         Sigma points and weights.
 
+    Examples
+    --------
+    >>> import numpy as np
+    >>> x = np.array([1.0, 2.0])
+    >>> P = np.eye(2) * 0.1
+    >>> sp = sigma_points_julier(x, P, kappa=1.0)
+    >>> sp.points.shape  # 2*n+1 = 5 points for n=2
+    (5, 2)
+    >>> np.allclose(sp.Wm.sum(), 1.0)
+    True
+
     Notes
     -----
     Julier's method is a special case of Merwe's with alpha=1, beta=0.
@@ -196,6 +207,21 @@ def unscented_transform(
         Weighted mean, shape (m,).
     cov : ndarray
         Weighted covariance, shape (m, m).
+
+    Examples
+    --------
+    >>> import numpy as np
+    >>> # Generate sigma points
+    >>> x = np.array([0.0, 0.0])
+    >>> P = np.eye(2)
+    >>> sp = sigma_points_merwe(x, P)
+    >>> # Pass through nonlinear function
+    >>> def f(x):
+    ...     return np.array([x[0]**2, x[1]])
+    >>> sigmas_f = np.array([f(sp.points[i]) for i in range(len(sp.points))])
+    >>> mean, cov = unscented_transform(sigmas_f, sp.Wm, sp.Wc)
+    >>> mean.shape
+    (2,)
     """
     # Weighted mean
     mean = np.sum(Wm[:, np.newaxis] * sigmas, axis=0)
@@ -325,6 +351,24 @@ def ukf_update(
     result : KalmanUpdate
         Updated state, covariance, and innovation statistics.
 
+    Examples
+    --------
+    Update with a range-bearing measurement:
+
+    >>> import numpy as np
+    >>> # State: [x, y], range-bearing measurement
+    >>> def h_rb(x):
+    ...     r = np.sqrt(x[0]**2 + x[1]**2)
+    ...     theta = np.arctan2(x[1], x[0])
+    ...     return np.array([r, theta])
+    >>> x = np.array([100.0, 50.0])
+    >>> P = np.eye(2) * 10.0
+    >>> z = np.array([112.0, 0.46])  # measured range and bearing
+    >>> R = np.diag([1.0, 0.01])  # measurement noise
+    >>> upd = ukf_update(x, P, z, h_rb, R)
+    >>> upd.x.shape
+    (2,)
+
     See Also
     --------
     ukf_predict : UKF prediction step.
@@ -400,6 +444,15 @@ def ckf_spherical_cubature_points(
     weights : ndarray
         Cubature weights.
 
+    Examples
+    --------
+    >>> import numpy as np
+    >>> points, weights = ckf_spherical_cubature_points(3)
+    >>> points.shape  # 2n = 6 points for n=3
+    (6, 3)
+    >>> np.allclose(weights.sum(), 1.0)
+    True
+
     Notes
     -----
     The CKF uses a third-degree spherical-radial cubature rule with
@@ -445,6 +498,20 @@ def ckf_predict(
     result : KalmanPrediction
         Predicted state and covariance.
 
+    Examples
+    --------
+    >>> import numpy as np
+    >>> # Linear dynamics: x_k+1 = F @ x_k
+    >>> def f(x):
+    ...     F = np.array([[1, 1], [0, 1]])
+    ...     return F @ x
+    >>> x = np.array([0.0, 1.0])
+    >>> P = np.eye(2) * 0.1
+    >>> Q = np.eye(2) * 0.01
+    >>> pred = ckf_predict(x, P, f, Q)
+    >>> pred.x  # Should be approximately [1, 1]
+    array([1., 1.])
+
     References
     ----------
     .. [1] Arasaratnam, I. and Haykin, S., "Cubature Kalman Filters",
@@ -512,6 +579,20 @@ def ckf_update(
     -------
     result : KalmanUpdate
         Updated state and covariance.
+
+    Examples
+    --------
+    >>> import numpy as np
+    >>> # Position measurement h(x) = x[0]
+    >>> def h(x):
+    ...     return np.array([x[0]])
+    >>> x = np.array([3.0, 1.0])  # predicted [position, velocity]
+    >>> P = np.eye(2) * 0.5
+    >>> z = np.array([3.1])  # measurement
+    >>> R = np.array([[0.1]])
+    >>> upd = ckf_update(x, P, z, h, R)
+    >>> upd.x.shape
+    (2,)
     """
     x = np.asarray(x, dtype=np.float64).flatten()
     P = np.asarray(P, dtype=np.float64)

pytcl/dynamic_estimation/particle_filters/bootstrap.py

@@ -48,6 +48,15 @@ def resample_multinomial(
     -------
     resampled : ndarray
         Resampled particles with uniform weights.
+
+    Examples
+    --------
+    >>> rng = np.random.default_rng(42)
+    >>> particles = np.array([[1.0], [2.0], [3.0], [4.0]])
+    >>> weights = np.array([0.1, 0.1, 0.1, 0.7])  # particle 4 dominant
+    >>> resampled = resample_multinomial(particles, weights, rng)
+    >>> resampled.shape
+    (4, 1)
     """
     if rng is None:
         rng = np.random.default_rng()
@@ -80,6 +89,15 @@ def resample_systematic(
     -------
     resampled : ndarray
         Resampled particles.
+
+    Examples
+    --------
+    >>> rng = np.random.default_rng(42)
+    >>> particles = np.array([[0.0], [1.0], [2.0], [3.0]])
+    >>> weights = np.array([0.25, 0.25, 0.25, 0.25])
+    >>> resampled = resample_systematic(particles, weights, rng)
+    >>> resampled.shape
+    (4, 1)
     """
     if rng is None:
         rng = np.random.default_rng()
@@ -145,6 +163,18 @@ def resample_residual(
     -------
     resampled : ndarray
         Resampled particles.
+
+    Examples
+    --------
+    >>> rng = np.random.default_rng(42)
+    >>> particles = np.array([[1.0], [2.0], [3.0], [4.0]])
+    >>> weights = np.array([0.1, 0.2, 0.3, 0.4])
+    >>> resampled = resample_residual(particles, weights, rng)
+    >>> resampled.shape
+    (4, 1)
+    >>> # High-weight particles are copied deterministically
+    >>> np.sum(resampled == 4.0) >= 1
+    True
     """
     if rng is None:
         rng = np.random.default_rng()
@@ -184,6 +214,17 @@ def effective_sample_size(weights: NDArray[np.floating]) -> float:
     ess : float
         Effective sample size, in range [1, N].
 
+    Examples
+    --------
+    >>> # Uniform weights -> ESS = N
+    >>> weights = np.array([0.25, 0.25, 0.25, 0.25])
+    >>> effective_sample_size(weights)
+    4.0
+    >>> # Degenerate weights -> ESS approaches 1
+    >>> weights = np.array([0.97, 0.01, 0.01, 0.01])
+    >>> effective_sample_size(weights)
+    1.06...
+
     Notes
     -----
     ESS = 1 / sum(w_i^2)
@@ -220,6 +261,29 @@ def bootstrap_pf_predict(
     -------
     particles_pred : ndarray
         Predicted particles.
+
+    Examples
+    --------
+    Predict particles through constant velocity dynamics:
+
+    >>> import numpy as np
+    >>> rng = np.random.default_rng(42)
+    >>> # 50 particles for 2D state [position, velocity]
+    >>> particles = np.column_stack([
+    ...     np.zeros(50),  # position
+    ...     np.ones(50)    # velocity = 1
+    ... ])
+    >>> dt = 0.1
+    >>> # x_next = [pos + vel*dt, vel]
+    >>> f = lambda x: np.array([x[0] + x[1] * dt, x[1]])
+    >>> # Process noise sampler
+    >>> Q_sample = lambda n, r: r.normal(0, 0.01, (n, 2))
+    >>> pred = bootstrap_pf_predict(particles, f, Q_sample, rng)
+    >>> pred.shape
+    (50, 2)
+    >>> # Positions moved forward by ~0.1
+    >>> np.mean(pred[:, 0])  # doctest: +ELLIPSIS
+    0.1...
     """
     if rng is None:
         rng = np.random.default_rng()
@@ -266,6 +330,25 @@ def bootstrap_pf_update(
         Updated normalized weights.
     log_likelihood : float
         Log marginal likelihood of measurement.
+
+    Examples
+    --------
+    Update particle weights with a position measurement:
+
+    >>> import numpy as np
+    >>> # 4 particles at different positions
+    >>> particles = np.array([[0.0], [1.0], [2.0], [3.0]])
+    >>> weights = np.ones(4) / 4  # uniform initial weights
+    >>> z = np.array([2.0])  # measured position
+    >>> # Gaussian likelihood centered on measurement
+    >>> def likelihood(z, x):
+    ...     return np.exp(-0.5 * (z[0] - x[0])**2)
+    >>> new_weights, log_lik = bootstrap_pf_update(particles, weights, z, likelihood)
+    >>> # Particle at x=2 should have highest weight
+    >>> np.argmax(new_weights)
+    2
+    >>> np.sum(new_weights)  # weights sum to 1
+    1.0
     """
     z = np.asarray(z, dtype=np.float64)
     N = len(particles)
@@ -312,6 +395,19 @@ def gaussian_likelihood(
     -------
     likelihood : float
         p(z|x) = N(z; z_pred, R).
+
+    Examples
+    --------
+    >>> z = np.array([1.0, 2.0])  # actual measurement
+    >>> z_pred = np.array([1.0, 2.0])  # perfect prediction
+    >>> R = np.eye(2) * 0.1  # measurement noise covariance
+    >>> lik = gaussian_likelihood(z, z_pred, R)
+    >>> lik > 0
+    True
+    >>> # Zero innovation -> maximum likelihood
+    >>> z_off = np.array([10.0, 10.0])
+    >>> gaussian_likelihood(z_off, z_pred, R) < lik
+    True
     """
     m = len(z)
     innovation = z - z_pred
@@ -366,6 +462,30 @@ def bootstrap_pf_step(
     -------
     state : ParticleState
         Updated particles and weights.
+
+    Examples
+    --------
+    Track a 1D random walk with position measurements:
+
+    >>> import numpy as np
+    >>> rng = np.random.default_rng(42)
+    >>> # 100 particles for 1D state
+    >>> particles = rng.normal(0, 1, (100, 1))
+    >>> weights = np.ones(100) / 100
+    >>> # Simple dynamics: x_k+1 = x_k + w
+    >>> f = lambda x: x
+    >>> h = lambda x: x  # measure position directly
+    >>> Q_sample = lambda n, r: r.normal(0, 0.1, (n, 1))
+    >>> R = np.array([[0.5]])  # measurement noise
+    >>> z = np.array([0.5])  # measurement
+    >>> state = bootstrap_pf_step(particles, weights, z, f, h, Q_sample, R, rng=rng)
+    >>> state.particles.shape
+    (100, 1)
+
+    See Also
+    --------
+    bootstrap_pf_predict : Prediction step only.
+    bootstrap_pf_update : Update step only.
     """
     if rng is None:
         rng = np.random.default_rng()
@@ -420,6 +540,13 @@ def particle_mean(
     -------
     mean : ndarray
         Weighted mean estimate.
+
+    Examples
+    --------
+    >>> particles = np.array([[0.0], [1.0], [2.0], [3.0]])
+    >>> weights = np.array([0.1, 0.2, 0.3, 0.4])
+    >>> particle_mean(particles, weights)
+    array([2.])
     """
     return np.sum(weights[:, np.newaxis] * particles, axis=0)
 
@@ -470,6 +597,14 @@ def particle_covariance(
     -------
     cov : ndarray
         Weighted covariance.
+
+    Examples
+    --------
+    >>> particles = np.array([[0.0, 0.0], [1.0, 0.0], [0.0, 1.0], [1.0, 1.0]])
+    >>> weights = np.array([0.25, 0.25, 0.25, 0.25])
+    >>> cov = particle_covariance(particles, weights)
+    >>> cov.shape
+    (2, 2)
     """
     if mean is None:
         mean = particle_mean(particles, weights)
@@ -505,6 +640,17 @@ def initialize_particles(
     -------
     state : ParticleState
         Initial particles with uniform weights.
+
+    Examples
+    --------
+    >>> rng = np.random.default_rng(42)
+    >>> x0 = np.array([0.0, 0.0])
+    >>> P0 = np.eye(2) * 0.1
+    >>> state = initialize_particles(x0, P0, N=100, rng=rng)
+    >>> state.particles.shape
+    (100, 2)
+    >>> np.allclose(state.weights, 0.01)  # uniform 1/N
+    True
     """
     if rng is None:
         rng = np.random.default_rng()

pytcl/dynamic_estimation/rbpf.py

@@ -501,6 +501,32 @@ def rbpf_predict(
     -------
     list[RBPFParticle]
         Predicted particles
+
+    Examples
+    --------
+    >>> import numpy as np
+    >>> from pytcl.dynamic_estimation.rbpf import RBPFParticle
+    >>> np.random.seed(42)
+    >>> # 3 particles with nonlinear bearing and linear position
+    >>> particles = [
+    ...     RBPFParticle(y=np.array([0.1]), x=np.array([0.0, 1.0]),
+    ...                  P=np.eye(2) * 0.5, w=1/3),
+    ...     RBPFParticle(y=np.array([0.0]), x=np.array([0.0, 1.0]),
+    ...                  P=np.eye(2) * 0.5, w=1/3),
+    ...     RBPFParticle(y=np.array([-0.1]), x=np.array([0.0, 1.0]),
+    ...                  P=np.eye(2) * 0.5, w=1/3),
+    ... ]
+    >>> # Nonlinear dynamics for bearing
+    >>> g = lambda y: y  # bearing stays constant
+    >>> G = np.eye(1)
+    >>> Qy = np.eye(1) * 0.01
+    >>> # Linear dynamics for position
+    >>> f = lambda x, y: np.array([x[0] + x[1] * 0.1, x[1]])
+    >>> F = np.array([[1, 0.1], [0, 1]])
+    >>> Qx = np.eye(2) * 0.01
+    >>> predicted = rbpf_predict(particles, g, G, Qy, f, F, Qx)
+    >>> len(predicted)
+    3
     """
     new_particles = []
 
@@ -553,6 +579,31 @@ def rbpf_update(
     -------
     list[RBPFParticle]
         Updated particles with adapted weights
+
+    Examples
+    --------
+    >>> import numpy as np
+    >>> from pytcl.dynamic_estimation.rbpf import RBPFParticle
+    >>> # 3 particles at different bearings
+    >>> particles = [
+    ...     RBPFParticle(y=np.array([0.5]), x=np.array([1.0, 0.0]),
+    ...                  P=np.eye(2) * 0.5, w=1/3),
+    ...     RBPFParticle(y=np.array([0.0]), x=np.array([1.0, 0.0]),
+    ...                  P=np.eye(2) * 0.5, w=1/3),
+    ...     RBPFParticle(y=np.array([-0.5]), x=np.array([1.0, 0.0]),
+    ...                  P=np.eye(2) * 0.5, w=1/3),
+    ... ]
+    >>> # Position measurement
+    >>> z = np.array([1.1])
+    >>> h = lambda x, y: np.array([x[0]])  # measure position
+    >>> H = np.array([[1, 0]])
+    >>> R = np.array([[0.1]])
+    >>> updated = rbpf_update(particles, z, h, H, R)
+    >>> len(updated)
+    3
+    >>> # Weights should sum to 1
+    >>> abs(sum(p.w for p in updated) - 1.0) < 1e-10
+    True
     """
     weights = np.zeros(len(particles))
     new_particles = []