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

pytcl/dynamic_estimation/gaussian_sum_filter.py

@@ -363,6 +363,33 @@ def gaussian_sum_filter_predict(
     -------
     components_new : list[GaussianComponent]
         Predicted components.
+
+    Examples
+    --------
+    >>> import numpy as np
+    >>> from pytcl.dynamic_estimation.gaussian_sum_filter import GaussianComponent
+    >>> # Two-component mixture for position-velocity state
+    >>> comp1 = GaussianComponent(
+    ...     x=np.array([0.0, 1.0]),  # moving right
+    ...     P=np.eye(2) * 0.5,
+    ...     w=0.5
+    ... )
+    >>> comp2 = GaussianComponent(
+    ...     x=np.array([0.0, -1.0]),  # moving left
+    ...     P=np.eye(2) * 0.5,
+    ...     w=0.5
+    ... )
+    >>> components = [comp1, comp2]
+    >>> # Constant velocity dynamics
+    >>> dt = 0.1
+    >>> f = lambda x: np.array([x[0] + x[1] * dt, x[1]])
+    >>> F = np.array([[1, dt], [0, 1]])
+    >>> Q = np.eye(2) * 0.01
+    >>> predicted = gaussian_sum_filter_predict(components, f, F, Q)
+    >>> len(predicted)
+    2
+    >>> predicted[0].w  # weights unchanged in prediction
+    0.5
     """
     F = np.asarray(F, dtype=np.float64)
     Q = np.asarray(Q, dtype=np.float64)
@@ -401,6 +428,34 @@ def gaussian_sum_filter_update(
     -------
     components_new : list[GaussianComponent]
         Updated components with adapted weights.
+
+    Examples
+    --------
+    >>> import numpy as np
+    >>> from pytcl.dynamic_estimation.gaussian_sum_filter import GaussianComponent
+    >>> # Two-component mixture
+    >>> comp1 = GaussianComponent(
+    ...     x=np.array([1.0, 0.5]),
+    ...     P=np.eye(2) * 0.5,
+    ...     w=0.5
+    ... )
+    >>> comp2 = GaussianComponent(
+    ...     x=np.array([3.0, 0.5]),
+    ...     P=np.eye(2) * 0.5,
+    ...     w=0.5
+    ... )
+    >>> components = [comp1, comp2]
+    >>> # Position measurement near component 1
+    >>> z = np.array([1.1])
+    >>> h = lambda x: np.array([x[0]])
+    >>> H = np.array([[1, 0]])
+    >>> R = np.array([[0.1]])
+    >>> updated = gaussian_sum_filter_update(components, z, h, H, R)
+    >>> len(updated)
+    2
+    >>> # Component 1 should have higher weight (closer to measurement)
+    >>> updated[0].w > updated[1].w
+    True
     """
     z = np.asarray(z, dtype=np.float64)
     H = np.asarray(H, dtype=np.float64)

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)