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

pytcl/navigation/great_circle.py

@@ -10,11 +10,20 @@ computing the shortest path on a sphere, including:
 - TDOA localization on a sphere
 """
 
-from typing import NamedTuple, Optional, Tuple
+import logging
+from functools import lru_cache
+from typing import Any, NamedTuple, Optional, Tuple
 
 import numpy as np
 from numpy.typing import NDArray
 
+# Module logger
+_logger = logging.getLogger("pytcl.navigation.great_circle")
+
+# Cache configuration for great circle calculations
+_GC_CACHE_DECIMALS = 10  # ~0.01mm precision at Earth's surface
+_GC_CACHE_MAXSIZE = 256  # Max cached coordinate pairs
+
 
 class GreatCircleResult(NamedTuple):
     """
@@ -96,6 +105,50 @@ class CrossTrackResult(NamedTuple):
 EARTH_RADIUS = 6371000.0
 
 
+def _quantize_coord(val: float) -> float:
+    """Quantize coordinate value for cache key compatibility."""
+    return round(val, _GC_CACHE_DECIMALS)
+
+
+@lru_cache(maxsize=_GC_CACHE_MAXSIZE)
+def _gc_distance_cached(
+    lat1_q: float,
+    lon1_q: float,
+    lat2_q: float,
+    lon2_q: float,
+) -> float:
+    """Cached great circle distance computation (internal).
+
+    Uses haversine formula for numerical stability.
+    Returns angular distance in radians.
+    """
+    dlat = lat2_q - lat1_q
+    dlon = lon2_q - lon1_q
+
+    a = np.sin(dlat / 2) ** 2 + np.cos(lat1_q) * np.cos(lat2_q) * np.sin(dlon / 2) ** 2
+    return 2 * np.arctan2(np.sqrt(a), np.sqrt(1 - a))
+
+
+@lru_cache(maxsize=_GC_CACHE_MAXSIZE)
+def _gc_azimuth_cached(
+    lat1_q: float,
+    lon1_q: float,
+    lat2_q: float,
+    lon2_q: float,
+) -> float:
+    """Cached great circle azimuth computation (internal).
+
+    Returns azimuth in radians [0, 2π).
+    """
+    dlon = lon2_q - lon1_q
+
+    x = np.sin(dlon) * np.cos(lat2_q)
+    y = np.cos(lat1_q) * np.sin(lat2_q) - np.sin(lat1_q) * np.cos(lat2_q) * np.cos(dlon)
+
+    azimuth = np.arctan2(x, y)
+    return azimuth % (2 * np.pi)
+
+
 def great_circle_distance(
     lat1: float,
     lon1: float,
@@ -107,6 +160,7 @@ def great_circle_distance(
     Compute great circle distance between two points.
 
     Uses the haversine formula for numerical stability at small distances.
+    Results are cached for repeated queries with the same coordinates.
 
     Parameters
     ----------
@@ -131,13 +185,14 @@ def great_circle_distance(
     >>> dist = great_circle_distance(lat1, lon1, lat2, lon2)
     >>> print(f"Distance: {dist/1000:.0f} km")
     """
-    dlat = lat2 - lat1
-    dlon = lon2 - lon1
-
-    a = np.sin(dlat / 2) ** 2 + np.cos(lat1) * np.cos(lat2) * np.sin(dlon / 2) ** 2
-    c = 2 * np.arctan2(np.sqrt(a), np.sqrt(1 - a))
-
-    return radius * c
+    # Use cached angular distance computation
+    angular_dist = _gc_distance_cached(
+        _quantize_coord(lat1),
+        _quantize_coord(lon1),
+        _quantize_coord(lat2),
+        _quantize_coord(lon2),
+    )
+    return radius * angular_dist
 
 
 def great_circle_azimuth(
@@ -149,6 +204,8 @@ def great_circle_azimuth(
     """
     Compute initial azimuth (bearing) from point 1 to point 2.
 
+    Results are cached for repeated queries with the same coordinates.
+
     Parameters
     ----------
     lat1, lon1 : float
@@ -170,15 +227,12 @@ def great_circle_azimuth(
     >>> az = great_circle_azimuth(lat1, lon1, lat2, lon2)
     >>> print(f"Initial bearing: {np.degrees(az):.1f}°")
     """
-    dlon = lon2 - lon1
-
-    x = np.sin(dlon) * np.cos(lat2)
-    y = np.cos(lat1) * np.sin(lat2) - np.sin(lat1) * np.cos(lat2) * np.cos(dlon)
-
-    azimuth = np.arctan2(x, y)
-
-    # Normalize to [0, 2π)
-    return azimuth % (2 * np.pi)
+    return _gc_azimuth_cached(
+        _quantize_coord(lat1),
+        _quantize_coord(lon1),
+        _quantize_coord(lat2),
+        _quantize_coord(lon2),
+    )
 
 
 def great_circle_inverse(
@@ -451,7 +505,7 @@ def great_circle_intersect(
     """
 
     # Convert to Cartesian unit vectors
-    def to_cartesian(lat, lon):
+    def to_cartesian(lat: Any, lon: Any) -> NDArray[np.float64]:
         return np.array(
             [np.cos(lat) * np.cos(lon), np.cos(lat) * np.sin(lon), np.sin(lat)]
         )
@@ -607,7 +661,7 @@ def great_circle_tdoa_loc(
     delta_d12 = delta_r12 / radius
     delta_d13 = delta_r13 / radius
 
-    def objective(lat, lon):
+    def objective(lat: Any, lon: Any) -> Any:
         """Objective function: difference between computed and observed TDOAs."""
         d1 = great_circle_distance(lat, lon, lat1, lon1, radius=1.0)
         d2 = great_circle_distance(lat, lon, lat2, lon2, radius=1.0)
@@ -771,6 +825,31 @@ def destination_point(
     return WaypointResult(float(lat2), float(lon2))
 
 
+def clear_great_circle_cache() -> None:
+    """Clear all great circle computation caches.
+
+    This can be useful to free memory after processing large datasets
+    or when cache statistics are being monitored.
+    """
+    _gc_distance_cached.cache_clear()
+    _gc_azimuth_cached.cache_clear()
+    _logger.debug("Great circle caches cleared")
+
+
+def get_cache_info() -> dict[str, Any]:
+    """Get cache statistics for great circle computations.
+
+    Returns
+    -------
+    dict[str, Any]
+        Dictionary with cache statistics for distance and azimuth caches.
+    """
+    return {
+        "distance": _gc_distance_cached.cache_info()._asdict(),
+        "azimuth": _gc_azimuth_cached.cache_info()._asdict(),
+    }
+
+
 __all__ = [
     # Constants
     "EARTH_RADIUS",
@@ -796,4 +875,7 @@ __all__ = [
     "great_circle_path_intersect",
     # TDOA
     "great_circle_tdoa_loc",
+    # Cache management
+    "clear_great_circle_cache",
+    "get_cache_info",
 ]

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/plotting/coordinates.py

@@ -5,10 +5,10 @@ This module provides functions for visualizing coordinate systems,
 rotations, and transformations in 2D and 3D.
 """
 
-from typing import List, Optional, Tuple
+from typing import Any, List, Optional, Tuple
 
 import numpy as np
-from numpy.typing import ArrayLike
+from numpy.typing import ArrayLike, NDArray
 
 try:
     import plotly.graph_objects as go
@@ -180,17 +180,17 @@ def plot_euler_angles(
     angles = np.asarray(angles)
 
     # Create rotation matrices for each axis
-    def rotx(a):
+    def rotx(a: Any) -> NDArray[np.float64]:
         return np.array(
             [[1, 0, 0], [0, np.cos(a), -np.sin(a)], [0, np.sin(a), np.cos(a)]]
         )
 
-    def roty(a):
+    def roty(a: Any) -> NDArray[np.float64]:
         return np.array(
             [[np.cos(a), 0, np.sin(a)], [0, 1, 0], [-np.sin(a), 0, np.cos(a)]]
         )
 
-    def rotz(a):
+    def rotz(a: Any) -> NDArray[np.float64]:
         return np.array(
             [[np.cos(a), -np.sin(a), 0], [np.sin(a), np.cos(a), 0], [0, 0, 1]]
         )
@@ -293,7 +293,7 @@ def plot_quaternion_interpolation(
     q_end = q_end / np.linalg.norm(q_end)
 
     # SLERP interpolation
-    def quat_slerp(q1, q2, t):
+    def quat_slerp(q1: Any, q2: Any, t: Any) -> NDArray[np.float64]:
         dot = np.dot(q1, q2)
         if dot < 0:
             q2 = -q2
@@ -303,7 +303,7 @@ def plot_quaternion_interpolation(
         theta = np.arccos(dot)
         return (np.sin((1 - t) * theta) * q1 + np.sin(t * theta) * q2) / np.sin(theta)
 
-    def quat_to_rotmat(q):
+    def quat_to_rotmat(q: Any) -> NDArray[np.float64]:
         w, x, y, z = q
         return np.array(
             [

pytcl/plotting/tracks.py

@@ -196,7 +196,7 @@ def plot_tracking_result(
     covariances: Optional[List[ArrayLike]] = None,
     x_idx: int = 0,
     y_idx: int = 2,
-    cov_xy_idx: tuple = (0, 2),
+    cov_xy_idx: tuple[int, int] = (0, 2),
     ellipse_interval: int = 5,
     n_std: float = 2.0,
     title: str = "Tracking Result",
@@ -596,7 +596,7 @@ def create_animated_tracking(
     covariances: Optional[List[ArrayLike]] = None,
     x_idx: int = 0,
     y_idx: int = 2,
-    cov_xy_idx: tuple = (0, 2),
+    cov_xy_idx: tuple[int, int] = (0, 2),
     n_std: float = 2.0,
     frame_duration: int = 100,
     title: str = "Animated Tracking",

pytcl/static_estimation/maximum_likelihood.py

@@ -12,7 +12,7 @@ References
        Wiley, 2001.
 """
 
-from typing import Callable, NamedTuple, Optional
+from typing import Any, Callable, NamedTuple, Optional
 
 import numpy as np
 from numpy.typing import ArrayLike, NDArray
@@ -75,10 +75,10 @@ class CRBResult(NamedTuple):
 
 
 def fisher_information_numerical(
-    log_likelihood: Callable[[NDArray], float],
+    log_likelihood: Callable[[np.ndarray[Any, Any]], float],
     theta: ArrayLike,
     h: float = 1e-5,
-) -> NDArray[np.floating]:
+) -> np.ndarray[Any, Any]:
     """
     Compute Fisher information matrix numerically.
 
@@ -189,11 +189,13 @@ def fisher_information_gaussian(
 
 
 def fisher_information_exponential_family(
-    sufficient_stats: Callable[[NDArray, NDArray], NDArray],
+    sufficient_stats: Callable[
+        [np.ndarray[Any, Any], np.ndarray[Any, Any]], np.ndarray[Any, Any]
+    ],
     theta: ArrayLike,
     data: ArrayLike,
     h: float = 1e-5,
-) -> NDArray[np.floating]:
+) -> np.ndarray[Any, Any]:
     """
     Fisher information for exponential family distributions.
 
@@ -232,10 +234,10 @@ def fisher_information_exponential_family(
 
 
 def observed_fisher_information(
-    log_likelihood: Callable[[NDArray], float],
+    log_likelihood: Callable[[np.ndarray[Any, Any]], float],
     theta: ArrayLike,
     h: float = 1e-5,
-) -> NDArray[np.floating]:
+) -> np.ndarray[Any, Any]:
     """
     Compute observed Fisher information (negative Hessian).
 
@@ -409,10 +411,10 @@ def efficiency(
 
 
 def mle_newton_raphson(
-    log_likelihood: Callable[[NDArray], float],
-    score: Callable[[NDArray], NDArray],
+    log_likelihood: Callable[[np.ndarray[Any, Any]], float],
+    score: Callable[[np.ndarray[Any, Any]], np.ndarray[Any, Any]],
     theta_init: ArrayLike,
-    hessian: Optional[Callable[[NDArray], NDArray]] = None,
+    hessian: Optional[Callable[[np.ndarray[Any, Any]], np.ndarray[Any, Any]]] = None,
     max_iter: int = 100,
     tol: float = 1e-8,
     h: float = 1e-5,
@@ -460,7 +462,7 @@ def mle_newton_raphson(
 
     converged = False
 
-    def numerical_hessian(t: NDArray) -> NDArray:
+    def numerical_hessian(t: np.ndarray[Any, Any]) -> np.ndarray[Any, Any]:
         H = np.zeros((n_params, n_params))
         for i in range(n_params):
             for j in range(i, n_params):
@@ -532,9 +534,9 @@ def mle_newton_raphson(
 
 
 def mle_scoring(
-    log_likelihood: Callable[[NDArray], float],
-    score: Callable[[NDArray], NDArray],
-    fisher_info: Callable[[NDArray], NDArray],
+    log_likelihood: Callable[[np.ndarray[Any, Any]], float],
+    score: Callable[[np.ndarray[Any, Any]], np.ndarray[Any, Any]],
+    fisher_info: Callable[[np.ndarray[Any, Any]], np.ndarray[Any, Any]],
     theta_init: ArrayLike,
     max_iter: int = 100,
     tol: float = 1e-8,

pytcl/static_estimation/robust.py

@@ -12,7 +12,7 @@ References
        Cartography," Communications of the ACM, 1981.
 """
 
-from typing import Callable, NamedTuple, Optional
+from typing import Any, Callable, NamedTuple, Optional
 
 import numpy as np
 from numpy.typing import ArrayLike, NDArray
@@ -313,8 +313,8 @@ def tau_scale(
 def irls(
     A: ArrayLike,
     b: ArrayLike,
-    weight_func: Callable[[NDArray], NDArray] = huber_weight,
-    scale_func: Callable[[NDArray], float] = mad,
+    weight_func: Callable[[np.ndarray[Any, Any]], np.ndarray[Any, Any]] = huber_weight,
+    scale_func: Callable[[np.ndarray[Any, Any]], float] = mad,
     max_iter: int = 50,
     tol: float = 1e-6,
 ) -> RobustResult:
@@ -455,7 +455,7 @@ def huber_regression(
     Gaussian errors and resistance to outliers.
     """
 
-    def weight_func(r: NDArray) -> NDArray:
+    def weight_func(r: np.ndarray[Any, Any]) -> np.ndarray[Any, Any]:
         return huber_weight(r, c)
 
     return irls(A, b, weight_func=weight_func, max_iter=max_iter, tol=tol)
@@ -504,7 +504,7 @@ def tukey_regression(
     Huber for gross outliers.
     """
 
-    def weight_func(r: NDArray) -> NDArray:
+    def weight_func(r: np.ndarray[Any, Any]) -> np.ndarray[Any, Any]:
         return tukey_weight(r, c)
 
     return irls(A, b, weight_func=weight_func, max_iter=max_iter, tol=tol)

pytcl/terrain/loaders.py

@@ -21,7 +21,7 @@ References
 import os
 from functools import lru_cache
 from pathlib import Path
-from typing import Dict, NamedTuple, Optional, Tuple
+from typing import Any, NamedTuple, Optional
 
 import numpy as np
 from numpy.typing import NDArray
@@ -31,7 +31,7 @@ from .dem import DEMGrid
 # Model parameters
 _GEBCO_BASE_URL = "https://www.gebco.net/data-products/gridded-bathymetry-data"
 
-GEBCO_PARAMETERS: Dict[str, Dict] = {
+GEBCO_PARAMETERS: dict[str, dict[str, Any]] = {
     "GEBCO2024": {
         "resolution_arcsec": 15.0,
         "n_lat": 43200,
@@ -58,7 +58,7 @@ GEBCO_PARAMETERS: Dict[str, Dict] = {
     },
 }
 
-EARTH2014_PARAMETERS: Dict[str, Dict] = {
+EARTH2014_PARAMETERS: dict[str, dict[str, Any]] = {
     "SUR": {
         "description": "Physical surface (topography, ice surface, 0 over oceans)",
         "file_pattern": "Earth2014.SUR2014.1min.geod.bin",
@@ -275,7 +275,7 @@ def parse_gebco_netcdf(
     lat_max: Optional[float] = None,
     lon_min: Optional[float] = None,
     lon_max: Optional[float] = None,
-) -> Tuple[NDArray, float, float, float, float]:
+) -> tuple[NDArray[np.floating], float, float, float, float]:
     """Parse GEBCO NetCDF file and extract region.
 
     Parameters
@@ -369,7 +369,7 @@ def parse_earth2014_binary(
     lat_max: Optional[float] = None,
     lon_min: Optional[float] = None,
     lon_max: Optional[float] = None,
-) -> Tuple[NDArray, float, float, float, float]:
+) -> tuple[NDArray[np.floating], float, float, float, float]:
     """Parse Earth2014 binary file and extract region.
 
     Earth2014 files are stored as int16 big-endian binary data,

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/hypothesis.py

@@ -155,7 +155,7 @@ def generate_joint_associations(
         track_idx: int,
         current: Dict[int, int],
         used_meas: Set[int],
-    ):
+    ) -> None:
         """Recursively enumerate associations."""
         if track_idx == n_tracks:
             associations.append(current.copy())

pytcl/trackers/mht.py

@@ -15,7 +15,7 @@ References
        IEEE Trans. Automatic Control, 1979.
 """
 
-from typing import Callable, Dict, List, NamedTuple, Optional, Set
+from typing import Callable, Dict, List, NamedTuple, Optional
 
 import numpy as np
 from numpy.typing import ArrayLike, NDArray
@@ -322,8 +322,8 @@ class MHTTracker:
     def _predict_tracks(
         self,
         tracks: Dict[int, MHTTrack],
-        F: NDArray,
-        Q: NDArray,
+        F: NDArray[np.floating],
+        Q: NDArray[np.floating],
     ) -> Dict[int, MHTTrack]:
         """Predict all tracks forward in time."""
         predicted = {}
@@ -352,8 +352,8 @@ class MHTTracker:
     def _compute_gating_and_likelihoods(
         self,
         tracks: Dict[int, MHTTrack],
-        Z: NDArray,
-    ) -> tuple[Set[tuple], Dict[tuple, float]]:
+        Z: NDArray[np.floating],
+    ) -> tuple[set[tuple[int, int]], dict[tuple[int, int], float]]:
         """Compute gating matrix and likelihood values."""
         gated = set()
         likelihood_matrix = {}
@@ -387,8 +387,8 @@ class MHTTracker:
         self,
         association: Dict[int, int],
         tracks: Dict[int, MHTTrack],
-        Z: NDArray,
-        likelihood_matrix: Dict[tuple, float],
+        Z: NDArray[np.floating],
+        likelihood_matrix: dict[tuple[int, int], float],
     ) -> float:
         """Compute likelihood of a joint association."""
         likelihood = 1.0
@@ -417,7 +417,7 @@ class MHTTracker:
     def _update_track(
         self,
         track: MHTTrack,
-        measurement: NDArray,
+        measurement: NDArray[np.floating],
         meas_idx: int,
     ) -> MHTTrack:
         """Update a track with a measurement."""
@@ -502,7 +502,7 @@ class MHTTracker:
 
     def _initiate_track(
         self,
-        measurement: NDArray,
+        measurement: NDArray[np.floating],
         meas_idx: int,
     ) -> MHTTrack:
         """Initiate a new track from a measurement."""

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):
@@ -228,7 +225,7 @@ class MultiTargetTracker:
                 track.covariance = F @ track.covariance @ F.T + Q
                 track.time = self._time
 
-    def _associate(self, Z: NDArray[np.float64]) -> dict:
+    def _associate(self, Z: NDArray[np.float64]) -> dict[int, int]:
         """
         Associate measurements to tracks using GNN.