nrl-tracker 0.21.1__py3-none-any.whl → 0.21.4__py3-none-any.whl

{nrl_tracker-0.21.1.dist-info → nrl_tracker-0.21.4.dist-info}/METADATA

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: nrl-tracker
-Version: 0.21.1
+Version: 0.21.4
 Summary: Python port of the U.S. Naval Research Laboratory's Tracker Component Library for target tracking algorithms
 Author: Original: David F. Crouse, Naval Research Laboratory
 Maintainer: Python Port Contributors
@@ -60,7 +60,7 @@ Requires-Dist: plotly>=5.15.0; extra == "visualization"
 
 # Tracker Component Library (Python)
 
-[![PyPI version](https://img.shields.io/badge/pypi-v0.21.1-blue.svg)](https://pypi.org/project/nrl-tracker/)
+[![PyPI version](https://img.shields.io/badge/pypi-v0.21.4-blue.svg)](https://pypi.org/project/nrl-tracker/)
 [![Python 3.10+](https://img.shields.io/badge/python-3.10+-blue.svg)](https://www.python.org/downloads/)
 [![License: Public Domain](https://img.shields.io/badge/License-Public%20Domain-brightgreen.svg)](https://en.wikipedia.org/wiki/Public_domain)
 [![Code style: black](https://img.shields.io/badge/code%20style-black-000000.svg)](https://github.com/psf/black)

{nrl_tracker-0.21.1.dist-info → nrl_tracker-0.21.4.dist-info}/RECORD

@@ -1,12 +1,12 @@
-pytcl/__init__.py,sha256=1grcwT7hqkF4MBmoUc9ugi2bhsecHLZuLZr8ALWSgC8,1894
+pytcl/__init__.py,sha256=gskiDd7_BZ-aWQ3h_uxnX6oGP3ylOmb69Mws0xLnEAQ,1894
 pytcl/assignment_algorithms/__init__.py,sha256=f9V-TkEVmiKYYyth4PTpDfJvA7yYV_ys6Zix-QwWIYY,2136
 pytcl/assignment_algorithms/data_association.py,sha256=X6Kww9-J2WLxU1790w7dRvAFdM90RSXHvs7IF26zweQ,11427
 pytcl/assignment_algorithms/gating.py,sha256=fN_oAOkv7nYYOWE1oPOLrcCn3xEpKdMVlFSbRMAURxY,10815
-pytcl/assignment_algorithms/jpda.py,sha256=hvZGkSrStuw9iQg7gPuIV7_bOinX3Xix5vLrVLMnEMM,19288
+pytcl/assignment_algorithms/jpda.py,sha256=Hv55j3J9qVwzlUfWdXdSasodTyB1ZKdgEpo5dBh95O8,19582
 pytcl/assignment_algorithms/three_dimensional/__init__.py,sha256=1Q40OUlUQoo7YKEucwdrSNo3D4A0Zibvkr8z4TpueBg,526
 pytcl/assignment_algorithms/three_dimensional/assignment.py,sha256=9BJhwlYu3JJ0kZ9sRyKKfpdvQdL4WYYHCtLbvaWycBw,19212
 pytcl/assignment_algorithms/two_dimensional/__init__.py,sha256=4Evsn__9hTfI2i8m8Ngl-Zy0Fa2OydKmDKlZlH6jaao,778
-pytcl/assignment_algorithms/two_dimensional/assignment.py,sha256=IRwbv-82dEbbm3KwjyUjuUWM3wP9s6oUtzX2mZffMjA,11419
+pytcl/assignment_algorithms/two_dimensional/assignment.py,sha256=eh87MBb-uiUSI1MXj4HrreRKB6Z8rxAyDkNQ8-u4SbM,11848
 pytcl/assignment_algorithms/two_dimensional/kbest.py,sha256=yiTToLuP7xWxQlQ8E-fpgXg-5iu0nnXcJXStjUB0nOE,17284
 pytcl/astronomical/__init__.py,sha256=SKELDaDhxpvCo1dMswBQYOQr_Th3ShuzZTzxZMdhE-U,5650
 pytcl/astronomical/lambert.py,sha256=Lc8FT1JmpI9WSXsG2s5vIRkSoBSV7r5hd3o2bGh2Ojo,15607
@@ -16,10 +16,10 @@ pytcl/astronomical/time_systems.py,sha256=Jg0Zaq60hc4Ts1aQtb5bK4KSZhz-uQse8gYC89
 pytcl/atmosphere/__init__.py,sha256=QAYgJYzgs0kreRV8fByii4p477LCxBDfrXB_cL7SYkM,706
 pytcl/atmosphere/models.py,sha256=pMLv8D7qoFqLZrlbTHLJJULOdDdhPskJ1m7KVKLV63E,9584
 pytcl/clustering/__init__.py,sha256=bYdhC_XJEt6KUUni9bIPxaddXNEGmIJQvGkA14rK4J8,1697
-pytcl/clustering/dbscan.py,sha256=cjBi5b0gcBgFqUtkLm1G0JMJLQYALahyYuoSI4G0zAY,6903
+pytcl/clustering/dbscan.py,sha256=PS6QlOwHFerbZNEb3zcNhN4oNQpgOOw5y0WskQzyKIo,7364
 pytcl/clustering/gaussian_mixture.py,sha256=U5U0Z46tZWdTLNdNNNJenoeviwZRAOvexVFYVLt4QMc,22865
-pytcl/clustering/hierarchical.py,sha256=PEkIGqAQ33InUGIcCRevv0rko4R1HqIwfejNhHsbnKQ,13794
-pytcl/clustering/kmeans.py,sha256=fmUIQdQLKCocwmMabdReQ0cKqMNKNFI_DyTCivPmAtw,10842
+pytcl/clustering/hierarchical.py,sha256=Hw9BFCn5df_ATpJX63R3B31MHz27ztCw9ihMDIlI688,14202
+pytcl/clustering/kmeans.py,sha256=250FQyDol5S_Y4TznNn9cEuE96UDp7wvEkPZJ1DLul8,10697
 pytcl/containers/__init__.py,sha256=t8oRtusBrh6G2dEk2PcofmxrpLPQ9nOJwH19GyKnNcc,1699
 pytcl/containers/cluster_set.py,sha256=_lZ39PNHTL7fUEZAwBF2ICK6v0GjZKpeUOg0knEdPzo,22760
 pytcl/containers/covertree.py,sha256=1JWqXxoUFLxuMnjwj2qf0iz2uPzdujQYdwJW3l5qsOs,13282
@@ -43,18 +43,18 @@ pytcl/core/array_utils.py,sha256=SsgEiAoRCWxAVKq1aa5-nPdOi-2AB6XNObu0IaGClUk,139
 pytcl/core/constants.py,sha256=lZVDK5zsSR02_4b2Nqx9KDtZT9QaYhkZ9wuoODbifd4,8693
 pytcl/core/validation.py,sha256=WRlzMlUihtqc3XZoWOTFK0sBAZVDIwTMGCiWcX5OZVY,13093
 pytcl/dynamic_estimation/__init__.py,sha256=jA5FF6kHYklY5LMOfZaKcCeiPTpVe8vHIMp3ECDOmsc,4582
-pytcl/dynamic_estimation/imm.py,sha256=CoOwJJv0DMrNKRkP-OqWbSxZO_-GAgNLaQ4KiBhjvEg,21313
+pytcl/dynamic_estimation/imm.py,sha256=t4dlutWeCLtAMl-ylNbxMUC3gbRBF_sXI3bX4PAk-OQ,22080
 pytcl/dynamic_estimation/information_filter.py,sha256=x7iQwO_iJT1dCSvDws5LqD3yAtjw9QVGUfMPcXn1IA4,17349
 pytcl/dynamic_estimation/smoothers.py,sha256=qC_g0YG0U4L_7rGBpcZzYcb11A--Hc8tb0fxWXIJdxM,18931
 pytcl/dynamic_estimation/batch_estimation/__init__.py,sha256=JQ0s76Enov5a7plA4EnUua4t-7etikQrwr5z4WIjUeo,46
 pytcl/dynamic_estimation/kalman/__init__.py,sha256=yoFLj0n-NRkdZnRVL-BkHBlATk8pfZEVlsY3BhSYgKc,2387
 pytcl/dynamic_estimation/kalman/extended.py,sha256=_deQTnUGOp_BlhP-FDEY0LOjgUMN32FQn0V12unCM4A,10397
-pytcl/dynamic_estimation/kalman/linear.py,sha256=sTfWt_yDxyQCA0SMjOx4xfhVnq3nReOVUObXuUuZRv8,11844
+pytcl/dynamic_estimation/kalman/linear.py,sha256=1Zgg9gZya0Vxs9im7sPUqLj0Luo463vS-RSa6GCReFI,12248
 pytcl/dynamic_estimation/kalman/square_root.py,sha256=Hw1F4_Zc7IA6Mt1WCkjx1UuLAUmNhM5vPLvueb7oRSA,26931
-pytcl/dynamic_estimation/kalman/unscented.py,sha256=NXPBTAf6814Yw5HTFpX21w_Y0eSC6q1gZSRUHHYEQjI,14943
+pytcl/dynamic_estimation/kalman/unscented.py,sha256=VmYE8LuM1nWpFTmD39iXdEZ3m41IsurCCbXJ19-ERFs,15440
 pytcl/dynamic_estimation/measurement_update/__init__.py,sha256=8rlyJwVpxf0fZj-AFo1hlewvryZRhUzcy3F8uMe6I8c,48
 pytcl/dynamic_estimation/particle_filters/__init__.py,sha256=-DRF5rVF2749suLlArmkTvVkqeMcV_mIx0eLeTj6wNU,906
-pytcl/dynamic_estimation/particle_filters/bootstrap.py,sha256=EfF9w0xqyBIYOHIwoYRt7D8aCgdPoGpvaFINu8gCZ80,12396
+pytcl/dynamic_estimation/particle_filters/bootstrap.py,sha256=FcF4W_NM5ZqJnw5fq4rq6fLY9X1r4uFJOiAX9a-NGG8,13371
 pytcl/dynamic_models/__init__.py,sha256=Cd8MyyYuB8gMnepkPA-HSwTaKFPThnqoKOhdjVOsXWg,2783
 pytcl/dynamic_models/continuous_time/__init__.py,sha256=dAkfEddLkfMvDalK9v2GRBvaZV1KgqYpFBLOnoiFClw,1023
 pytcl/dynamic_models/continuous_time/dynamics.py,sha256=CDwqn-66eUwXA5xfIjaG6A4EDBqtOyQ3aWarJr9QH4g,12858
@@ -141,8 +141,8 @@ pytcl/trackers/mht.py,sha256=7mwhMmja3ri2wnx7W1wueDGn2r3ArwAxJDPUJ7IZAkQ,20617
 pytcl/trackers/multi_target.py,sha256=7ZL8V25TO_rEMtQm2eYkScesDQHC9qXZVHLHyVbxy3M,10529
 pytcl/trackers/single_target.py,sha256=Yy3FwaNTArMWcaod-0HVeiioNV4xLWxNDn_7ZPVqQYs,6562
 pytcl/transponders/__init__.py,sha256=5fL4u3lKCYgPLo5uFeuZbtRZkJPABntuKYGUvVgMMEI,41
-nrl_tracker-0.21.1.dist-info/LICENSE,sha256=rB5G4WppIIUzMOYr2N6uyYlNJ00hRJqE5tie6BMvYuE,1612
-nrl_tracker-0.21.1.dist-info/METADATA,sha256=Do-PM_-vvQ08w8QWNyvIuRmsAEqzFXe8NSO5BXLTlHg,10005
-nrl_tracker-0.21.1.dist-info/WHEEL,sha256=pL8R0wFFS65tNSRnaOVrsw9EOkOqxLrlUPenUYnJKNo,91
-nrl_tracker-0.21.1.dist-info/top_level.txt,sha256=17megxcrTPBWwPZTh6jTkwTKxX7No-ZqRpyvElnnO-s,6
-nrl_tracker-0.21.1.dist-info/RECORD,,
+nrl_tracker-0.21.4.dist-info/LICENSE,sha256=rB5G4WppIIUzMOYr2N6uyYlNJ00hRJqE5tie6BMvYuE,1612
+nrl_tracker-0.21.4.dist-info/METADATA,sha256=kfdnUMvBqNlpI3skCRZZQIQF8gYBLHSGcgSZ5gGwDXw,10005
+nrl_tracker-0.21.4.dist-info/WHEEL,sha256=pL8R0wFFS65tNSRnaOVrsw9EOkOqxLrlUPenUYnJKNo,91
+nrl_tracker-0.21.4.dist-info/top_level.txt,sha256=17megxcrTPBWwPZTh6jTkwTKxX7No-ZqRpyvElnnO-s,6
+nrl_tracker-0.21.4.dist-info/RECORD,,

pytcl/__init__.py

@@ -20,7 +20,7 @@ References
        no. 5, pp. 18-27, May 2017.
 """
 
-__version__ = "0.18.0"
+__version__ = "0.21.4"
 __author__ = "Python Port Contributors"
 __original_author__ = "David F. Crouse, Naval Research Laboratory"
 

pytcl/assignment_algorithms/jpda.py

@@ -12,6 +12,7 @@ as JPDA can handle measurement origin uncertainty in cluttered environments.
 from typing import List, NamedTuple, Optional, Tuple
 
 import numpy as np
+from numba import njit
 from numpy.typing import ArrayLike, NDArray
 from scipy.stats import chi2
 
@@ -314,45 +315,34 @@ def _jpda_exact(
     return beta
 
 
-def _jpda_approximate(
-    likelihood_matrix: NDArray,
-    gated: NDArray,
+@njit(cache=True)
+def _jpda_approximate_core(
+    likelihood_matrix: np.ndarray,
+    gated: np.ndarray,
     detection_prob: float,
     clutter_density: float,
-) -> NDArray:
-    """
-    Approximate JPDA using parametric approach.
-
-    Uses the approach from [1] which is O(n_tracks * n_meas^2).
-
-    References
-    ----------
-    .. [1] Fitzgerald, R.J., "Development of Practical PDA Logic for
-           Multitarget Tracking by Microprocessor", American Control
-           Conference, 1986.
-    """
-    n_tracks, n_meas = likelihood_matrix.shape
-    beta = np.zeros((n_tracks, n_meas + 1))
-
-    # For each track, compute association probabilities independently
-    # then apply correction for shared measurements
+) -> np.ndarray:
+    """JIT-compiled core of approximate JPDA computation."""
+    n_tracks = likelihood_matrix.shape[0]
+    n_meas = likelihood_matrix.shape[1]
+    beta = np.zeros((n_tracks, n_meas + 1), dtype=np.float64)
 
     # Likelihood ratio for each measurement given each track
-    # L[i,j] = likelihood(z_j | track i) / clutter_density
-    L = np.zeros((n_tracks, n_meas))
+    L = np.zeros((n_tracks, n_meas), dtype=np.float64)
     for i in range(n_tracks):
         for j in range(n_meas):
             if gated[i, j] and clutter_density > 0:
                 L[i, j] = likelihood_matrix[i, j] / clutter_density
             elif gated[i, j]:
-                L[i, j] = likelihood_matrix[i, j] * 1e10  # Large value
+                L[i, j] = likelihood_matrix[i, j] * 1e10
 
     # Compute delta factors (accounts for other tracks)
-    delta = np.ones((n_tracks, n_meas))
+    delta = np.ones((n_tracks, n_meas), dtype=np.float64)
 
     for j in range(n_meas):
-        # Sum of likelihood ratios for measurement j
-        sum_L = np.sum(L[:, j])
+        sum_L = 0.0
+        for i in range(n_tracks):
+            sum_L += L[i, j]
         for i in range(n_tracks):
             if sum_L > 0:
                 delta[i, j] = 1.0 / (1.0 + sum_L - L[i, j])
@@ -361,7 +351,6 @@ def _jpda_approximate(
 
     # Compute association probabilities
     for i in range(n_tracks):
-        # Denominator for normalization
         denom = 1.0 - detection_prob
 
         for j in range(n_meas):
@@ -369,9 +358,9 @@ def _jpda_approximate(
                 beta[i, j] = detection_prob * L[i, j] * delta[i, j]
                 denom += beta[i, j]
 
-        # Normalize
         if denom > 0:
-            beta[i, :n_meas] /= denom
+            for j in range(n_meas):
+                beta[i, j] /= denom
             beta[i, n_meas] = (1.0 - detection_prob) / denom
         else:
             beta[i, n_meas] = 1.0
@@ -379,6 +368,31 @@ def _jpda_approximate(
     return beta
 
 
+def _jpda_approximate(
+    likelihood_matrix: NDArray,
+    gated: NDArray,
+    detection_prob: float,
+    clutter_density: float,
+) -> NDArray:
+    """
+    Approximate JPDA using parametric approach.
+
+    Uses the approach from [1] which is O(n_tracks * n_meas^2).
+
+    References
+    ----------
+    .. [1] Fitzgerald, R.J., "Development of Practical PDA Logic for
+           Multitarget Tracking by Microprocessor", American Control
+           Conference, 1986.
+    """
+    return _jpda_approximate_core(
+        likelihood_matrix.astype(np.float64),
+        gated.astype(np.bool_),
+        detection_prob,
+        clutter_density,
+    )
+
+
 def jpda_update(
     track_states: List[ArrayLike],
     track_covariances: List[ArrayLike],

pytcl/assignment_algorithms/two_dimensional/assignment.py

@@ -211,14 +211,21 @@ def auction(
             # Compute values: value[j] = -cost[i,j] - prices[j]
             values = -cost[i, :] - prices
 
-            # Find best and second best
-            sorted_idx = np.argsort(values)[::-1]
-            best_j = sorted_idx[0]
-            best_value = values[best_j]
-
-            if len(sorted_idx) > 1:
-                second_value = values[sorted_idx[1]]
+            # Find best and second best using argpartition (O(n) vs O(n log n))
+            if len(values) >= 2:
+                # Get indices of top 2 values
+                top2_idx = np.argpartition(values, -2)[-2:]
+                # Determine which is best and second best
+                if values[top2_idx[0]] > values[top2_idx[1]]:
+                    best_j = top2_idx[0]
+                    second_value = values[top2_idx[1]]
+                else:
+                    best_j = top2_idx[1]
+                    second_value = values[top2_idx[0]]
+                best_value = values[best_j]
             else:
+                best_j = np.argmax(values)
+                best_value = values[best_j]
                 second_value = -np.inf
 
             # Bid increment

pytcl/clustering/dbscan.py

@@ -15,6 +15,7 @@ References
 from typing import List, NamedTuple, Set
 
 import numpy as np
+from numba import njit
 from numpy.typing import ArrayLike, NDArray
 
 
@@ -40,6 +41,23 @@ class DBSCANResult(NamedTuple):
     n_noise: int
 
 
+@njit(cache=True)
+def _compute_distance_matrix(X: np.ndarray) -> np.ndarray:
+    """Compute pairwise Euclidean distance matrix (JIT-compiled)."""
+    n = X.shape[0]
+    dist = np.zeros((n, n), dtype=np.float64)
+    for i in range(n):
+        for j in range(i + 1, n):
+            d = 0.0
+            for k in range(X.shape[1]):
+                diff = X[i, k] - X[j, k]
+                d += diff * diff
+            d = np.sqrt(d)
+            dist[i, j] = d
+            dist[j, i] = d
+    return dist
+
+
 def compute_neighbors(
     X: NDArray[np.floating],
     eps: float,
@@ -60,13 +78,13 @@ def compute_neighbors(
         neighbors[i] contains indices of points within eps of point i.
     """
     n_samples = X.shape[0]
-    neighbors = []
 
+    # Use JIT-compiled distance matrix computation
+    dist_matrix = _compute_distance_matrix(X)
+
+    neighbors = []
     for i in range(n_samples):
-        # Compute distances from point i to all points
-        distances = np.sqrt(np.sum((X - X[i]) ** 2, axis=1))
-        # Find points within eps (including self)
-        neighbor_indices = np.where(distances <= eps)[0]
+        neighbor_indices = np.where(dist_matrix[i] <= eps)[0]
         neighbors.append(neighbor_indices)
 
     return neighbors

pytcl/clustering/hierarchical.py

@@ -15,6 +15,7 @@ from enum import Enum
 from typing import List, Literal, NamedTuple, Optional
 
 import numpy as np
+from numba import njit
 from numpy.typing import ArrayLike, NDArray
 
 
@@ -70,6 +71,26 @@ class HierarchicalResult(NamedTuple):
     dendrogram: List[DendrogramNode]
 
 
+@njit(cache=True)
+def _compute_distance_matrix_jit(X: np.ndarray) -> np.ndarray:
+    """JIT-compiled pairwise Euclidean distance computation."""
+    n = X.shape[0]
+    n_features = X.shape[1]
+    distances = np.zeros((n, n), dtype=np.float64)
+
+    for i in range(n):
+        for j in range(i + 1, n):
+            d = 0.0
+            for k in range(n_features):
+                diff = X[i, k] - X[j, k]
+                d += diff * diff
+            d = np.sqrt(d)
+            distances[i, j] = d
+            distances[j, i] = d
+
+    return distances
+
+
 def compute_distance_matrix(
     X: NDArray[np.floating],
 ) -> NDArray[np.floating]:
@@ -86,16 +107,8 @@ def compute_distance_matrix(
     distances : ndarray
         Distance matrix, shape (n_samples, n_samples).
     """
-    n = X.shape[0]
-    distances = np.zeros((n, n))
-
-    for i in range(n):
-        for j in range(i + 1, n):
-            d = np.sqrt(np.sum((X[i] - X[j]) ** 2))
-            distances[i, j] = d
-            distances[j, i] = d
-
-    return distances
+    X = np.asarray(X, dtype=np.float64)
+    return _compute_distance_matrix_jit(X)
 
 
 def _single_linkage(

pytcl/clustering/kmeans.py

@@ -14,6 +14,7 @@ from typing import Literal, NamedTuple, Optional, Union
 
 import numpy as np
 from numpy.typing import ArrayLike, NDArray
+from scipy.spatial.distance import cdist
 
 
 class KMeansResult(NamedTuple):
@@ -91,11 +92,8 @@ def kmeans_plusplus_init(
 
     # Subsequent centers: sample proportional to D^2
     for k in range(1, n_clusters):
-        # Compute squared distances to nearest center
-        distances_sq = np.full(n_samples, np.inf)
-        for j in range(k):
-            d_sq = np.sum((X - centers[j]) ** 2, axis=1)
-            distances_sq = np.minimum(distances_sq, d_sq)
+        # Compute squared distances to nearest center (vectorized via cdist)
+        distances_sq = cdist(X, centers[:k], metric="sqeuclidean").min(axis=1)
 
         # Sample proportional to D^2
         probs = distances_sq / distances_sq.sum()
@@ -138,12 +136,9 @@ def assign_clusters(
     centers = np.asarray(centers, dtype=np.float64)
 
     n_samples = X.shape[0]
-    n_clusters = centers.shape[0]
 
-    # Compute distances to all centers
-    distances_sq = np.zeros((n_samples, n_clusters))
-    for k in range(n_clusters):
-        distances_sq[:, k] = np.sum((X - centers[k]) ** 2, axis=1)
+    # Compute squared distances to all centers (vectorized via cdist)
+    distances_sq = cdist(X, centers, metric="sqeuclidean")
 
     # Assign to nearest center
     labels = np.argmin(distances_sq, axis=1).astype(np.intp)

pytcl/dynamic_estimation/imm.py

@@ -131,15 +131,16 @@ def compute_mixing_probabilities(
     # Predicted mode probabilities: c_bar[j] = sum_i Pi[i,j] * mu[i]
     c_bar = Pi.T @ mode_probs
 
-    # Mixing probabilities: mu[i|j] = Pi[i,j] * mu[i] / c_bar[j]
-    mixing_probs = np.zeros((r, r))
-    for j in range(r):
-        if c_bar[j] > 1e-15:
-            for i in range(r):
-                mixing_probs[i, j] = Pi[i, j] * mode_probs[i] / c_bar[j]
-        else:
-            # Uniform if predicted probability is zero
-            mixing_probs[:, j] = 1.0 / r
+    # Mixing probabilities: mu[i|j] = Pi[i,j] * mu[i] / c_bar[j] (vectorized)
+    # Compute numerator: Pi[i,j] * mu[i] for all i,j
+    numerator = Pi * mode_probs[:, np.newaxis]
+    # Divide by c_bar (with safe division for near-zero values)
+    safe_c_bar = np.where(c_bar > 1e-15, c_bar, 1.0)
+    mixing_probs = numerator / safe_c_bar
+    # Set uniform for columns where c_bar was too small
+    zero_mask = c_bar <= 1e-15
+    if np.any(zero_mask):
+        mixing_probs[:, zero_mask] = 1.0 / r
 
     return mixing_probs, c_bar
 
@@ -169,23 +170,30 @@ def mix_states(
         Mixed covariances for each mode.
     """
     r = len(mode_states)
-    n = len(mode_states[0])
+
+    # Stack states and covariances for vectorized operations
+    states_array = np.array(mode_states)  # shape (r, n)
+    covs_array = np.array(mode_covs)  # shape (r, n, n)
 
     mixed_states = []
     mixed_covs = []
 
     for j in range(r):
-        # Mixed state: x_0j = sum_i mu[i|j] * x_i
-        x_mixed = np.zeros(n)
-        for i in range(r):
-            x_mixed += mixing_probs[i, j] * mode_states[i]
-        mixed_states.append(x_mixed)
+        # Mixed state: x_0j = sum_i mu[i|j] * x_i (vectorized)
+        x_mixed = mixing_probs[:, j] @ states_array
 
         # Mixed covariance: P_0j = sum_i mu[i|j] * (P_i + (x_i - x_0j)(x_i - x_0j)^T)
-        P_mixed = np.zeros((n, n))
-        for i in range(r):
-            diff = mode_states[i] - x_mixed
-            P_mixed += mixing_probs[i, j] * (mode_covs[i] + np.outer(diff, diff))
+        # Compute differences for all modes at once
+        diffs = states_array - x_mixed  # shape (r, n)
+        # Weighted covariances + outer products (vectorized)
+        weights = mixing_probs[:, j]
+        # Weighted sum of covariances
+        P_mixed = np.tensordot(weights, covs_array, axes=([0], [0]))
+        # Add weighted outer products: sum_i w_i * outer(diff_i, diff_i)
+        weighted_diffs = np.sqrt(weights)[:, np.newaxis] * diffs
+        P_mixed += weighted_diffs.T @ weighted_diffs
+
+        mixed_states.append(x_mixed)
         mixed_covs.append(P_mixed)
 
     return mixed_states, mixed_covs
@@ -215,19 +223,20 @@ def combine_estimates(
     P : ndarray
         Combined covariance.
     """
-    r = len(mode_states)
-    n = len(mode_states[0])
-
-    # Combined state: x = sum_j mu_j * x_j
-    x = np.zeros(n)
-    for j in range(r):
-        x += mode_probs[j] * mode_states[j]
-
-    # Combined covariance: P = sum_j mu_j * (P_j + (x_j - x)(x_j - x)^T)
-    P = np.zeros((n, n))
-    for j in range(r):
-        diff = mode_states[j] - x
-        P += mode_probs[j] * (mode_covs[j] + np.outer(diff, diff))
+    # Stack states and covariances for vectorized operations
+    states_array = np.array(mode_states)  # shape (r, n)
+    covs_array = np.array(mode_covs)  # shape (r, n, n)
+
+    # Combined state: x = sum_j mu_j * x_j (vectorized)
+    x = mode_probs @ states_array
+
+    # Combined covariance: P = sum_j mu_j * (P_j + (x_j - x)(x_j - x)^T) (vectorized)
+    diffs = states_array - x  # shape (r, n)
+    # Weighted sum of covariances
+    P = np.tensordot(mode_probs, covs_array, axes=([0], [0]))
+    # Add weighted outer products
+    weighted_diffs = np.sqrt(mode_probs)[:, np.newaxis] * diffs
+    P += weighted_diffs.T @ weighted_diffs
 
     # Ensure symmetry
     P = (P + P.T) / 2

pytcl/dynamic_estimation/kalman/linear.py

@@ -9,6 +9,7 @@ from typing import NamedTuple, Optional, Tuple
 
 import numpy as np
 from numpy.typing import ArrayLike, NDArray
+from scipy.linalg import cho_factor, cho_solve
 
 
 class KalmanState(NamedTuple):
@@ -204,9 +205,22 @@ def kf_update(
     # Innovation covariance
     S = H @ P @ H.T + R
 
-    # Kalman gain using solve for numerical stability
-    # K = P @ H' @ S^{-1}
-    K = np.linalg.solve(S.T, H @ P.T).T
+    # Use Cholesky decomposition for efficient solving (reused for gain and likelihood)
+    # This is more numerically stable and efficient than repeated solve() calls
+    try:
+        S_cho = cho_factor(S)
+        # Kalman gain: K = P @ H' @ S^{-1}
+        K = cho_solve(S_cho, H @ P.T).T
+        # Mahalanobis distance for likelihood
+        mahal_sq = y @ cho_solve(S_cho, y)
+        # Log determinant from Cholesky factor (more stable than det)
+        log_det_S = 2 * np.sum(np.log(np.diag(S_cho[0])))
+        m = len(z)
+        likelihood = np.exp(-0.5 * (mahal_sq + log_det_S + m * np.log(2 * np.pi)))
+    except np.linalg.LinAlgError:
+        # Fallback if Cholesky fails (S not positive definite)
+        K = np.linalg.solve(S.T, H @ P.T).T
+        likelihood = 0.0
 
     # Updated state
     x_upd = x + K @ y
@@ -218,16 +232,6 @@ def kf_update(
     # Ensure symmetry
     P_upd = (P_upd + P_upd.T) / 2
 
-    # Compute likelihood for data association
-    # p(z|x) = N(z; H@x, S)
-    m = len(z)
-    det_S = np.linalg.det(S)
-    if det_S > 0:
-        mahal_sq = y @ np.linalg.solve(S, y)
-        likelihood = np.exp(-0.5 * mahal_sq) / np.sqrt((2 * np.pi) ** m * det_S)
-    else:
-        likelihood = 0.0
-
     return KalmanUpdate(
         x=x_upd,
         P=P_upd,

pytcl/dynamic_estimation/kalman/unscented.py

@@ -203,11 +203,19 @@ def unscented_transform(
     # Weighted mean
     mean = np.sum(Wm[:, np.newaxis] * sigmas, axis=0)
 
-    # Weighted covariance
+    # Weighted covariance (vectorized: avoids loop over sigma points)
     residuals = sigmas - mean
-    cov = np.zeros((sigmas.shape[1], sigmas.shape[1]), dtype=np.float64)
-    for i in range(len(sigmas)):
-        cov += Wc[i] * np.outer(residuals[i], residuals[i])
+    # Compute weighted outer products in one operation: (W * residuals)^T @ residuals
+    weighted_residuals = np.sqrt(np.abs(Wc))[:, np.newaxis] * residuals
+    # Handle negative weights (e.g., from Merwe scaling) by adjusting sign
+    cov = weighted_residuals.T @ weighted_residuals
+    # Correct for any negative weights (subtract their contribution twice to flip sign)
+    neg_mask = Wc < 0
+    if np.any(neg_mask):
+        neg_residuals = residuals[neg_mask]
+        neg_weights = -Wc[neg_mask]
+        for i, (w, r) in enumerate(zip(neg_weights, neg_residuals)):
+            cov -= 2 * w * np.outer(r, r)
 
     if noise_cov is not None:
         cov += np.asarray(noise_cov, dtype=np.float64)
@@ -329,7 +337,6 @@ def ukf_update(
     z = np.asarray(z, dtype=np.float64).flatten()
     R = np.asarray(R, dtype=np.float64)
 
-    n = len(x)
     m = len(z)
 
     # Generate sigma points
@@ -341,10 +348,11 @@ def ukf_update(
     # Predicted measurement mean and covariance
     z_pred, S = unscented_transform(sigmas_h, sp.Wm, sp.Wc, R)
 
-    # Cross-covariance between state and measurement
-    Pxz = np.zeros((n, m), dtype=np.float64)
-    for i in range(len(sp.points)):
-        Pxz += sp.Wc[i] * np.outer(sp.points[i] - x, sigmas_h[i] - z_pred)
+    # Cross-covariance between state and measurement (vectorized)
+    x_residuals = sp.points - x
+    z_residuals = sigmas_h - z_pred
+    # Weighted cross-covariance: sum of Wc[i] * outer(x_res[i], z_res[i])
+    Pxz = (sp.Wc[:, np.newaxis] * x_residuals).T @ z_residuals
 
     # Kalman gain
     K = np.linalg.solve(S.T, Pxz.T).T
@@ -467,12 +475,11 @@ def ckf_predict(
     # Predicted mean
     x_pred = np.sum(weights[:, np.newaxis] * transformed, axis=0)
 
-    # Predicted covariance
+    # Predicted covariance (vectorized)
     residuals = transformed - x_pred
-    P_pred = np.zeros((n, n), dtype=np.float64)
-    for i in range(len(cubature_pts)):
-        P_pred += weights[i] * np.outer(residuals[i], residuals[i])
-    P_pred += Q
+    # All CKF weights are equal and positive, so vectorization is straightforward
+    weighted_residuals = np.sqrt(weights)[:, np.newaxis] * residuals
+    P_pred = weighted_residuals.T @ weighted_residuals + Q
 
     P_pred = (P_pred + P_pred.T) / 2
 
@@ -533,18 +540,14 @@ def ckf_update(
     # Predicted measurement
     z_pred = np.sum(weights[:, np.newaxis] * transformed, axis=0)
 
-    # Innovation covariance
+    # Innovation covariance (vectorized)
     z_residuals = transformed - z_pred
-    S = np.zeros((m, m), dtype=np.float64)
-    for i in range(len(cubature_pts)):
-        S += weights[i] * np.outer(z_residuals[i], z_residuals[i])
-    S += R
+    weighted_z_residuals = np.sqrt(weights)[:, np.newaxis] * z_residuals
+    S = weighted_z_residuals.T @ weighted_z_residuals + R
 
-    # Cross-covariance
+    # Cross-covariance (vectorized)
     x_residuals = cubature_pts - x
-    Pxz = np.zeros((n, m), dtype=np.float64)
-    for i in range(len(cubature_pts)):
-        Pxz += weights[i] * np.outer(x_residuals[i], z_residuals[i])
+    Pxz = (weights[:, np.newaxis] * x_residuals).T @ z_residuals
 
     # Kalman gain
     K = np.linalg.solve(S.T, Pxz.T).T

pytcl/dynamic_estimation/particle_filters/bootstrap.py

@@ -8,6 +8,7 @@ state estimation.
 from typing import Callable, NamedTuple, Optional, Tuple
 
 import numpy as np
+from numba import njit
 from numpy.typing import ArrayLike, NDArray
 
 
@@ -99,6 +100,27 @@ def resample_systematic(
     return particles[indices].copy()
 
 
+@njit(cache=True)
+def _resample_residual_deterministic(
+    particles: np.ndarray,
+    floor_Nw: np.ndarray,
+) -> Tuple[np.ndarray, int]:
+    """JIT-compiled deterministic copy portion of residual resampling."""
+    N = particles.shape[0]
+    n = particles.shape[1]
+    resampled = np.zeros((N, n), dtype=np.float64)
+
+    idx = 0
+    for i in range(N):
+        count = floor_Nw[i]
+        for _ in range(count):
+            for k in range(n):
+                resampled[idx, k] = particles[i, k]
+            idx += 1
+
+    return resampled, idx
+
+
 def resample_residual(
     particles: NDArray[np.floating],
     weights: NDArray[np.floating],
@@ -128,24 +150,16 @@ def resample_residual(
         rng = np.random.default_rng()
 
     N = len(weights)
-    n = particles.shape[1]
 
     # Integer and fractional parts
     Nw = N * weights
-    floor_Nw = np.floor(Nw).astype(int)
+    floor_Nw = np.floor(Nw).astype(np.int64)
     residual = Nw - floor_Nw
 
-    # Number of deterministic copies (used implicitly via floor_Nw loop)
-
-    # Allocate output
-    resampled = np.zeros((N, n), dtype=np.float64)
-
-    # Deterministic copies
-    idx = 0
-    for i in range(N):
-        for _ in range(floor_Nw[i]):
-            resampled[idx] = particles[i]
-            idx += 1
+    # Deterministic copies (JIT-compiled)
+    resampled, idx = _resample_residual_deterministic(
+        particles.astype(np.float64), floor_Nw
+    )
 
     # Multinomial resampling of residuals
     if idx < N:
@@ -410,6 +424,31 @@ def particle_mean(
     return np.sum(weights[:, np.newaxis] * particles, axis=0)
 
 
+@njit(cache=True)
+def _particle_covariance_core(
+    particles: np.ndarray,
+    weights: np.ndarray,
+    mean: np.ndarray,
+) -> np.ndarray:
+    """JIT-compiled core for particle covariance computation."""
+    N = particles.shape[0]
+    n = particles.shape[1]
+    cov = np.zeros((n, n), dtype=np.float64)
+
+    for i in range(N):
+        w = weights[i]
+        for j in range(n):
+            diff_j = particles[i, j] - mean[j]
+            for k in range(j, n):
+                diff_k = particles[i, k] - mean[k]
+                val = w * diff_j * diff_k
+                cov[j, k] += val
+                if j != k:
+                    cov[k, j] += val
+
+    return cov
+
+
 def particle_covariance(
     particles: NDArray[np.floating],
     weights: NDArray[np.floating],
@@ -435,12 +474,11 @@ def particle_covariance(
     if mean is None:
         mean = particle_mean(particles, weights)
 
-    residuals = particles - mean
-    cov = np.zeros((particles.shape[1], particles.shape[1]), dtype=np.float64)
-    for i in range(len(particles)):
-        cov += weights[i] * np.outer(residuals[i], residuals[i])
-
-    return cov
+    return _particle_covariance_core(
+        particles.astype(np.float64),
+        weights.astype(np.float64),
+        mean.astype(np.float64),
+    )
 
 
 def initialize_particles(