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

pytcl/coordinate_systems/projections/__init__.py

@@ -26,8 +26,10 @@ Examples
 ...                         result.zone, result.hemisphere)
 """
 
-from pytcl.coordinate_systems.projections.projections import (  # Constants; Result types; Azimuthal Equidistant; UTM; Lambert Conformal Conic; Mercator; Stereographic; Transverse Mercator
-    WGS84_A,
+from pytcl.coordinate_systems.projections.projections import (
+    WGS84_A,  # Constants; Result types; Azimuthal Equidistant; UTM; Lambert Conformal Conic; Mercator; Stereographic; Transverse Mercator
+)
+from pytcl.coordinate_systems.projections.projections import (
     WGS84_B,
     WGS84_E,
     WGS84_E2,

pytcl/dynamic_estimation/imm.py

@@ -17,10 +17,7 @@ from typing import List, NamedTuple, Optional
 import numpy as np
 from numpy.typing import ArrayLike, NDArray
 
-from pytcl.dynamic_estimation.kalman.linear import (
-    kf_predict,
-    kf_update,
-)
+from pytcl.dynamic_estimation.kalman.linear import kf_predict, kf_update
 
 
 class IMMState(NamedTuple):
@@ -131,15 +128,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 +167,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 +220,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/extended.py

@@ -10,10 +10,7 @@ from typing import Callable
 import numpy as np
 from numpy.typing import ArrayLike, NDArray
 
-from pytcl.dynamic_estimation.kalman.linear import (
-    KalmanPrediction,
-    KalmanUpdate,
-)
+from pytcl.dynamic_estimation.kalman.linear import KalmanPrediction, KalmanUpdate
 
 
 def ekf_predict(

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

@@ -10,10 +10,7 @@ from typing import Callable, NamedTuple, Optional, Tuple
 import numpy as np
 from numpy.typing import ArrayLike, NDArray
 
-from pytcl.dynamic_estimation.kalman.linear import (
-    KalmanPrediction,
-    KalmanUpdate,
-)
+from pytcl.dynamic_estimation.kalman.linear import KalmanPrediction, KalmanUpdate
 
 
 class SigmaPoints(NamedTuple):
@@ -203,11 +200,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 +334,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 +345,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 +472,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 +537,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(

pytcl/dynamic_estimation/smoothers.py

@@ -17,11 +17,7 @@ from typing import List, NamedTuple, Optional
 import numpy as np
 from numpy.typing import ArrayLike, NDArray
 
-from pytcl.dynamic_estimation.kalman.linear import (
-    kf_predict,
-    kf_smooth,
-    kf_update,
-)
+from pytcl.dynamic_estimation.kalman.linear import kf_predict, kf_smooth, kf_update
 
 
 class SmoothedState(NamedTuple):

pytcl/dynamic_models/discrete_time/__init__.py

@@ -17,11 +17,7 @@ from pytcl.dynamic_models.discrete_time.polynomial import (
     f_piecewise_white_noise_jerk,
     f_poly_kal,
 )
-from pytcl.dynamic_models.discrete_time.singer import (
-    f_singer,
-    f_singer_2d,
-    f_singer_3d,
-)
+from pytcl.dynamic_models.discrete_time.singer import f_singer, f_singer_2d, f_singer_3d
 
 __all__ = [
     # Polynomial models

pytcl/dynamic_models/process_noise/__init__.py

@@ -17,11 +17,7 @@ from pytcl.dynamic_models.process_noise.polynomial import (
     q_discrete_white_noise,
     q_poly_kal,
 )
-from pytcl.dynamic_models.process_noise.singer import (
-    q_singer,
-    q_singer_2d,
-    q_singer_3d,
-)
+from pytcl.dynamic_models.process_noise.singer import q_singer, q_singer_2d, q_singer_3d
 
 __all__ = [
     # Polynomial models

pytcl/magnetism/__init__.py

@@ -25,22 +25,11 @@ Examples
 >>> coef = create_emm_test_coefficients(n_max=36)
 """
 
-from pytcl.magnetism.emm import (
-    EMM_PARAMETERS,
-    HighResCoefficients,
-)
+from pytcl.magnetism.emm import EMM_PARAMETERS, HighResCoefficients
 from pytcl.magnetism.emm import create_test_coefficients as create_emm_test_coefficients
-from pytcl.magnetism.emm import (
-    emm,
-    emm_declination,
-    emm_inclination,
-    emm_intensity,
-)
+from pytcl.magnetism.emm import emm, emm_declination, emm_inclination, emm_intensity
 from pytcl.magnetism.emm import get_data_dir as get_emm_data_dir
-from pytcl.magnetism.emm import (
-    load_emm_coefficients,
-    wmmhr,
-)
+from pytcl.magnetism.emm import load_emm_coefficients, wmmhr
 from pytcl.magnetism.igrf import (
     IGRF13,
     IGRFModel,

pytcl/mathematical_functions/interpolation/__init__.py

@@ -8,8 +8,8 @@ This module provides:
 - Spherical interpolation
 """
 
-from pytcl.mathematical_functions.interpolation.interpolation import (  # noqa: E501
-    akima,
+from pytcl.mathematical_functions.interpolation.interpolation import akima  # noqa: E501
+from pytcl.mathematical_functions.interpolation.interpolation import (
     barycentric,
     cubic_spline,
     interp1d,

pytcl/mathematical_functions/special_functions/__init__.py

@@ -40,8 +40,8 @@ from pytcl.mathematical_functions.special_functions.debye import (
     debye_entropy,
     debye_heat_capacity,
 )
-from pytcl.mathematical_functions.special_functions.elliptic import (  # noqa: E501
-    ellipe,
+from pytcl.mathematical_functions.special_functions.elliptic import ellipe  # noqa: E501
+from pytcl.mathematical_functions.special_functions.elliptic import (
     ellipeinc,
     ellipk,
     ellipkinc,

pytcl/navigation/__init__.py

@@ -11,8 +11,10 @@ needed in tracking applications, including:
 - Rhumb line navigation
 """
 
-from pytcl.navigation.geodesy import (  # Ellipsoids; Coordinate conversions; Geodetic problems
-    GRS80,
+from pytcl.navigation.geodesy import (
+    GRS80,  # Ellipsoids; Coordinate conversions; Geodetic problems
+)
+from pytcl.navigation.geodesy import (
     SPHERE,
     WGS84,
     Ellipsoid,
@@ -26,8 +28,8 @@ from pytcl.navigation.geodesy import (  # Ellipsoids; Coordinate conversions; Ge
     inverse_geodetic,
     ned_to_ecef,
 )
-from pytcl.navigation.great_circle import (  # Great circle navigation
-    EARTH_RADIUS,
+from pytcl.navigation.great_circle import EARTH_RADIUS  # Great circle navigation
+from pytcl.navigation.great_circle import (
     CrossTrackResult,
     GreatCircleResult,
     IntersectionResult,
@@ -45,8 +47,10 @@ from pytcl.navigation.great_circle import (  # Great circle navigation
     great_circle_waypoint,
     great_circle_waypoints,
 )
-from pytcl.navigation.ins import (  # Constants; State representation; Gravity and Earth rate
-    A_EARTH,
+from pytcl.navigation.ins import (
+    A_EARTH,  # Constants; State representation; Gravity and Earth rate
+)
+from pytcl.navigation.ins import (
     B_EARTH,
     E2_EARTH,
     F_EARTH,
@@ -73,8 +77,8 @@ from pytcl.navigation.ins import (  # Constants; State representation; Gravity a
     update_attitude_ned,
     update_quaternion,
 )
-from pytcl.navigation.ins_gnss import (  # INS/GNSS integration
-    GPS_L1_FREQ,
+from pytcl.navigation.ins_gnss import GPS_L1_FREQ  # INS/GNSS integration
+from pytcl.navigation.ins_gnss import (
     GPS_L1_WAVELENGTH,
     SPEED_OF_LIGHT,
     GNSSMeasurement,
@@ -99,8 +103,8 @@ from pytcl.navigation.ins_gnss import (  # INS/GNSS integration
     tight_coupled_update,
     velocity_measurement_matrix,
 )
-from pytcl.navigation.rhumb import (  # Rhumb line navigation
-    RhumbDirectResult,
+from pytcl.navigation.rhumb import RhumbDirectResult  # Rhumb line navigation
+from pytcl.navigation.rhumb import (
     RhumbIntersectionResult,
     RhumbResult,
     compare_great_circle_rhumb,

pytcl/navigation/ins.py

@@ -23,11 +23,7 @@ from typing import NamedTuple, Optional, Tuple
 import numpy as np
 from numpy.typing import ArrayLike, NDArray
 
-from pytcl.coordinate_systems.rotations import (
-    quat2rotmat,
-    quat_multiply,
-    rotmat2quat,
-)
+from pytcl.coordinate_systems.rotations import quat2rotmat, quat_multiply, rotmat2quat
 from pytcl.navigation.geodesy import WGS84, Ellipsoid
 
 # =============================================================================

pytcl/trackers/__init__.py

@@ -16,20 +16,9 @@ from pytcl.trackers.hypothesis import (
     n_scan_prune,
     prune_hypotheses_by_probability,
 )
-from pytcl.trackers.mht import (
-    MHTConfig,
-    MHTResult,
-    MHTTracker,
-)
-from pytcl.trackers.multi_target import (
-    MultiTargetTracker,
-    Track,
-    TrackStatus,
-)
-from pytcl.trackers.single_target import (
-    SingleTargetTracker,
-    TrackState,
-)
+from pytcl.trackers.mht import MHTConfig, MHTResult, MHTTracker
+from pytcl.trackers.multi_target import MultiTargetTracker, Track, TrackStatus
+from pytcl.trackers.single_target import SingleTargetTracker, TrackState
 
 __all__ = [
     # Single target

pytcl/trackers/multi_target.py

@@ -11,10 +11,7 @@ from typing import Callable, List, NamedTuple, Optional
 import numpy as np
 from numpy.typing import ArrayLike, NDArray
 
-from pytcl.assignment_algorithms import (
-    chi2_gate_threshold,
-    gnn_association,
-)
+from pytcl.assignment_algorithms import chi2_gate_threshold, gnn_association
 
 
 class TrackStatus(Enum):