nrl-tracker 0.21.5__py3-none-any.whl → 0.22.1__py3-none-any.whl

pytcl/navigation/great_circle.py

@@ -344,9 +344,7 @@ def great_circle_direct(
     """
     d = distance / radius  # Angular distance
 
-    lat2 = np.arcsin(
-        np.sin(lat1) * np.cos(d) + np.cos(lat1) * np.sin(d) * np.cos(azimuth)
-    )
+    lat2 = np.arcsin(np.sin(lat1) * np.cos(d) + np.cos(lat1) * np.sin(d) * np.cos(azimuth))
 
     lon2 = lon1 + np.arctan2(
         np.sin(azimuth) * np.sin(d) * np.cos(lat1),
@@ -408,9 +406,7 @@ def cross_track_distance(
     # Along-track distance
     dat = np.arccos(np.cos(d13) / np.cos(dxt))
 
-    return CrossTrackResult(
-        cross_track=float(dxt * radius), along_track=float(dat * radius)
-    )
+    return CrossTrackResult(cross_track=float(dxt * radius), along_track=float(dat * radius))
 
 
 def great_circle_intersect(
@@ -452,9 +448,7 @@ def great_circle_intersect(
 
     # Convert to Cartesian unit vectors
     def to_cartesian(lat, lon):
-        return np.array(
-            [np.cos(lat) * np.cos(lon), np.cos(lat) * np.sin(lon), np.sin(lat)]
-        )
+        return np.array([np.cos(lat) * np.cos(lon), np.cos(lat) * np.sin(lon), np.sin(lat)])
 
     # Normal vectors to the great circles
     p1 = to_cartesian(lat1, lon1)
@@ -508,9 +502,7 @@ def great_circle_intersect(
     lat_i2 = -lat_i1
     lon_i2 = ((lon_i1 + np.pi) % (2 * np.pi)) - np.pi
 
-    return IntersectionResult(
-        float(lat_i1), float(lon_i1), float(lat_i2), float(lon_i2), True
-    )
+    return IntersectionResult(float(lat_i1), float(lon_i1), float(lat_i2), float(lon_i2), True)
 
 
 def great_circle_path_intersect(

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
 
 # =============================================================================
@@ -248,11 +244,7 @@ def normal_gravity(lat: float, alt: float = 0.0) -> float:
 
     # Free-air correction (first-order)
     g = g0 * (
-        1
-        - 2
-        * alt
-        / A_EARTH
-        * (1 + F_EARTH + (OMEGA_EARTH**2 * A_EARTH**2 * B_EARTH) / GM_EARTH)
+        1 - 2 * alt / A_EARTH * (1 + F_EARTH + (OMEGA_EARTH**2 * A_EARTH**2 * B_EARTH) / GM_EARTH)
     )
 
     return g
@@ -576,9 +568,7 @@ def update_quaternion(
 
     if angle < 1e-10:
         # Small angle approximation
-        delta_q = np.array(
-            [1.0, 0.5 * delta_theta[0], 0.5 * delta_theta[1], 0.5 * delta_theta[2]]
-        )
+        delta_q = np.array([1.0, 0.5 * delta_theta[0], 0.5 * delta_theta[1], 0.5 * delta_theta[2]])
     else:
         # Exact quaternion for rotation
         axis = delta_theta / angle

pytcl/navigation/ins_gnss.py

@@ -512,9 +512,7 @@ def loose_coupled_predict(
     dt = imu.dt
 
     # Propagate INS mechanization
-    ins_new = mechanize_ins_ned(
-        state.ins_state, imu, accel_prev=accel_prev, gyro_prev=gyro_prev
-    )
+    ins_new = mechanize_ins_ned(state.ins_state, imu, accel_prev=accel_prev, gyro_prev=gyro_prev)
 
     # Get error state transition matrix (continuous-time)
     F_cont = ins_error_state_matrix(state.ins_state)
@@ -576,9 +574,7 @@ def loose_coupled_update_position(
     if gnss.position_cov is not None:
         R = gnss.position_cov
     else:
-        R = np.diag(
-            [10.0**2, 10.0**2, 15.0**2]
-        )  # Default: 10m horizontal, 15m vertical
+        R = np.diag([10.0**2, 10.0**2, 15.0**2])  # Default: 10m horizontal, 15m vertical
 
     # Innovation: measured position - INS predicted position
     z = gnss.position - state.ins_state.position
@@ -695,16 +691,8 @@ def loose_coupled_update(
         # Full position + velocity update
         H = position_velocity_measurement_matrix()
 
-        R_pos = (
-            gnss.position_cov
-            if gnss.position_cov is not None
-            else np.diag([10.0**2] * 3)
-        )
-        R_vel = (
-            gnss.velocity_cov
-            if gnss.velocity_cov is not None
-            else np.diag([0.1**2] * 3)
-        )
+        R_pos = gnss.position_cov if gnss.position_cov is not None else np.diag([10.0**2] * 3)
+        R_vel = gnss.velocity_cov if gnss.velocity_cov is not None else np.diag([0.1**2] * 3)
         R = np.block([[R_pos, np.zeros((3, 3))], [np.zeros((3, 3)), R_vel]])
 
         z = np.concatenate(
@@ -846,9 +834,7 @@ def tight_coupled_measurement_matrix(
         los, _ = compute_line_of_sight(user_ecef, sat.position)
 
         # LOS components in ECEF
-        los_x, los_y, los_z = (
-            -los
-        )  # Negative because increase in user pos decreases range
+        los_x, los_y, los_z = -los  # Negative because increase in user pos decreases range
 
         # Transform LOS to geodetic derivatives
         # d(range)/d(lat), d(range)/d(lon), d(range)/d(alt)
@@ -858,9 +844,7 @@ def tight_coupled_measurement_matrix(
             + los_z * (cos_lat * N * (1 - ellipsoid.e2))
         )
         H[i, 1] = los_x * (-cos_lat * sin_lon * N) + los_y * (cos_lat * cos_lon * N)
-        H[i, 2] = (
-            los_x * cos_lat * cos_lon + los_y * cos_lat * sin_lon + los_z * sin_lat
-        )
+        H[i, 2] = los_x * cos_lat * cos_lon + los_y * cos_lat * sin_lon + los_z * sin_lat
 
         # Clock bias (state 15)
         H[i, 15] = 1.0
@@ -991,9 +975,7 @@ def _apply_error_correction(
 
     # Apply small angle rotation to quaternion
     q = ins_state.quaternion
-    delta_q = np.array(
-        [1.0, 0.5 * phi[0], 0.5 * phi[1], 0.5 * phi[2]], dtype=np.float64
-    )
+    delta_q = np.array([1.0, 0.5 * phi[0], 0.5 * phi[1], 0.5 * phi[2]], dtype=np.float64)
     delta_q = delta_q / np.linalg.norm(delta_q)
 
     # Quaternion multiplication (body frame correction)

pytcl/navigation/rhumb.py

@@ -100,9 +100,7 @@ def _isometric_latitude(lat: float, e2: float = 0.0) -> float:
         )
 
 
-def _inverse_isometric_latitude(
-    psi: float, e2: float = 0.0, max_iter: int = 20
-) -> float:
+def _inverse_isometric_latitude(psi: float, e2: float = 0.0, max_iter: int = 20) -> float:
     """
     Compute geodetic latitude from isometric latitude.
 
@@ -131,10 +129,7 @@ def _inverse_isometric_latitude(
         for _ in range(max_iter):
             sin_lat = np.sin(lat)
             lat_new = (
-                2
-                * np.arctan(
-                    ((1 + e * sin_lat) / (1 - e * sin_lat)) ** (e / 2) * np.exp(psi)
-                )
+                2 * np.arctan(((1 + e * sin_lat) / (1 - e * sin_lat)) ** (e / 2) * np.exp(psi))
                 - np.pi / 2
             )
             if abs(lat_new - lat) < 1e-12:
@@ -427,9 +422,7 @@ def indirect_rhumb(
     if abs(dlon) > np.pi:
         dlon = dlon - np.sign(dlon) * 2 * np.pi
 
-    dpsi = _isometric_latitude(lat2, ellipsoid.e2) - _isometric_latitude(
-        lat1, ellipsoid.e2
-    )
+    dpsi = _isometric_latitude(lat2, ellipsoid.e2) - _isometric_latitude(lat1, ellipsoid.e2)
     bearing = np.arctan2(dlon, dpsi) % (2 * np.pi)
 
     return RhumbResult(distance, bearing)

pytcl/performance_evaluation/track_metrics.py

@@ -158,9 +158,7 @@ def ospa(
     loc_component = (localization_sum / n) ** (1.0 / p) if localization_sum > 0 else 0.0
     card_component = (cardinality_penalty / n) ** (1.0 / p) if n > m else 0.0
 
-    return OSPAResult(
-        ospa=ospa_val, localization=loc_component, cardinality=card_component
-    )
+    return OSPAResult(ospa=ospa_val, localization=loc_component, cardinality=card_component)
 
 
 def ospa_over_time(

pytcl/plotting/coordinates.py

@@ -181,19 +181,13 @@ def plot_euler_angles(
 
     # Create rotation matrices for each axis
     def rotx(a):
-        return np.array(
-            [[1, 0, 0], [0, np.cos(a), -np.sin(a)], [0, np.sin(a), np.cos(a)]]
-        )
+        return np.array([[1, 0, 0], [0, np.cos(a), -np.sin(a)], [0, np.sin(a), np.cos(a)]])
 
     def roty(a):
-        return np.array(
-            [[np.cos(a), 0, np.sin(a)], [0, 1, 0], [-np.sin(a), 0, np.cos(a)]]
-        )
+        return np.array([[np.cos(a), 0, np.sin(a)], [0, 1, 0], [-np.sin(a), 0, np.cos(a)]])
 
     def rotz(a):
-        return np.array(
-            [[np.cos(a), -np.sin(a), 0], [np.sin(a), np.cos(a), 0], [0, 0, 1]]
-        )
+        return np.array([[np.cos(a), -np.sin(a), 0], [np.sin(a), np.cos(a), 0], [0, 0, 1]])
 
     rot_funcs = {"X": rotx, "Y": roty, "Z": rotz}
 
@@ -247,11 +241,7 @@ def plot_euler_angles(
     for i in range(4):
         scene_name = f"scene{i + 1}" if i > 0 else "scene"
         fig.update_layout(
-            **{
-                scene_name: dict(
-                    aspectmode="cube", camera=dict(eye=dict(x=1.5, y=1.5, z=1.5))
-                )
-            }
+            **{scene_name: dict(aspectmode="cube", camera=dict(eye=dict(x=1.5, y=1.5, z=1.5)))}
         )
 
     return fig
@@ -378,9 +368,7 @@ def plot_quaternion_interpolation(
                         method="animate",
                         args=[
                             [None],
-                            dict(
-                                frame=dict(duration=0, redraw=False), mode="immediate"
-                            ),
+                            dict(frame=dict(duration=0, redraw=False), mode="immediate"),
                         ],
                     ),
                 ],

pytcl/plotting/metrics.py

@@ -148,9 +148,7 @@ def plot_nees_sequence(
     fig.add_trace(
         go.Scatter(
             x=np.concatenate([time, time[::-1]]),
-            y=np.concatenate(
-                [np.full(n_steps, upper_bound), np.full(n_steps, lower_bound)]
-            ),
+            y=np.concatenate([np.full(n_steps, upper_bound), np.full(n_steps, lower_bound)]),
             fill="toself",
             fillcolor="rgba(0, 255, 0, 0.1)",
             line=dict(color="rgba(0,0,0,0)"),
@@ -534,9 +532,7 @@ def plot_consistency_summary(
     fig.add_trace(
         go.Scatter(
             x=np.concatenate([time, time[::-1]]),
-            y=np.concatenate(
-                [np.full(n_steps, nees_upper), np.full(n_steps, nees_lower)]
-            ),
+            y=np.concatenate([np.full(n_steps, nees_upper), np.full(n_steps, nees_lower)]),
             fill="toself",
             fillcolor="rgba(0, 255, 0, 0.1)",
             line=dict(color="rgba(0,0,0,0)"),
@@ -580,9 +576,7 @@ def plot_consistency_summary(
         fig.add_trace(
             go.Scatter(
                 x=np.concatenate([time, time[::-1]]),
-                y=np.concatenate(
-                    [np.full(n_steps, nis_upper), np.full(n_steps, nis_lower)]
-                ),
+                y=np.concatenate([np.full(n_steps, nis_upper), np.full(n_steps, nis_lower)]),
                 fill="toself",
                 fillcolor="rgba(0, 255, 0, 0.1)",
                 line=dict(color="rgba(0,0,0,0)"),

pytcl/plotting/tracks.py

@@ -369,11 +369,7 @@ def plot_multi_target_tracks(
 
     for idx, (track_id, states) in enumerate(tracks.items()):
         states = np.asarray(states)
-        color = (
-            colors.get(track_id)
-            if colors
-            else default_colors[idx % len(default_colors)]
-        )
+        color = colors.get(track_id) if colors else default_colors[idx % len(default_colors)]
 
         fig.add_trace(
             go.Scatter(
@@ -536,9 +532,7 @@ def plot_estimation_comparison(
 
         # Error bounds
         if covariances is not None:
-            sigma = n_std * np.array(
-                [np.sqrt(P[state_idx, state_idx]) for P in covariances]
-            )
+            sigma = n_std * np.array([np.sqrt(P[state_idx, state_idx]) for P in covariances])
             upper = estimates[:, state_idx] + sigma
             lower = estimates[:, state_idx] - sigma
 
@@ -740,9 +734,7 @@ def create_animated_tracking(
                         method="animate",
                         args=[
                             [None],
-                            dict(
-                                frame=dict(duration=0, redraw=False), mode="immediate"
-                            ),
+                            dict(frame=dict(duration=0, redraw=False), mode="immediate"),
                         ],
                     ),
                 ],

pytcl/static_estimation/least_squares.py

@@ -303,8 +303,7 @@ def total_least_squares(
     # The solution exists if V[n, n] != 0
     if abs(V[n, n]) < 1e-14:
         raise ValueError(
-            "TLS solution does not exist. The smallest singular value "
-            "has multiplicity > 1."
+            "TLS solution does not exist. The smallest singular value " "has multiplicity > 1."
         )
 
     # TLS solution: x = -V[0:n, n] / V[n, n]

pytcl/static_estimation/maximum_likelihood.py

@@ -654,9 +654,9 @@ def mle_gaussian(
         theta = np.array(theta)
 
         # Log-likelihood
-        log_lik = -n / 2 * np.log(2 * np.pi * var_mle) - np.sum(
-            (data - mean_mle) ** 2
-        ) / (2 * var_mle)
+        log_lik = -n / 2 * np.log(2 * np.pi * var_mle) - np.sum((data - mean_mle) ** 2) / (
+            2 * var_mle
+        )
 
         # Fisher information
         n_params = len(theta)

pytcl/terrain/dem.py

@@ -193,9 +193,7 @@ class DEMGrid:
 
     def _in_bounds(self, lat: float, lon: float) -> bool:
         """Check if coordinates are within DEM bounds."""
-        return (
-            self.lat_min <= lat <= self.lat_max and self.lon_min <= lon <= self.lon_max
-        )
+        return self.lat_min <= lat <= self.lat_max and self.lon_min <= lon <= self.lon_max
 
     def _get_indices(self, lat: float, lon: float) -> Tuple[int, int, float, float]:
         """Get grid indices and fractional parts for interpolation.
@@ -275,11 +273,7 @@ class DEMGrid:
             z00 = self.data[i, j]
             z01 = self.data[i, j + 1] if j + 1 < self.n_lon else z00
             z10 = self.data[i + 1, j] if i + 1 < self.n_lat else z00
-            z11 = (
-                self.data[i + 1, j + 1]
-                if (i + 1 < self.n_lat and j + 1 < self.n_lon)
-                else z00
-            )
+            z11 = self.data[i + 1, j + 1] if (i + 1 < self.n_lat and j + 1 < self.n_lon) else z00
 
             # Check for nodata
             values = [z00, z01, z10, z11]

pytcl/terrain/loaders.py

@@ -308,8 +308,7 @@ def parse_gebco_netcdf(
         import netCDF4 as nc
     except ImportError:
         raise ImportError(
-            "netCDF4 is required for loading GEBCO files.\n"
-            "Install with: pip install netCDF4"
+            "netCDF4 is required for loading GEBCO files.\n" "Install with: pip install netCDF4"
         )
 
     # Set defaults for global extent
@@ -432,13 +431,9 @@ def parse_earth2014_binary(
 
     # Compute row/column indices
     i_start = max(0, int(np.floor((np.radians(lat_min_deg) - lat_start) / d_lat)))
-    i_end = min(
-        EARTH2014_N_LAT, int(np.ceil((np.radians(lat_max_deg) - lat_start) / d_lat)) + 1
-    )
+    i_end = min(EARTH2014_N_LAT, int(np.ceil((np.radians(lat_max_deg) - lat_start) / d_lat)) + 1)
     j_start = max(0, int(np.floor((np.radians(lon_min_deg) - lon_start) / d_lon)))
-    j_end = min(
-        EARTH2014_N_LON, int(np.ceil((np.radians(lon_max_deg) - lon_start) / d_lon)) + 1
-    )
+    j_end = min(EARTH2014_N_LON, int(np.ceil((np.radians(lon_max_deg) - lon_start) / d_lon)) + 1)
 
     # Read binary data
     # File is stored as int16 big-endian, rows from south to north
@@ -455,9 +450,7 @@ def parse_earth2014_binary(
             f.seek(row_offset + col_offset)
 
             # Read row segment
-            row_data = np.frombuffer(
-                f.read(n_cols * 2), dtype=">i2"
-            )  # big-endian int16
+            row_data = np.frombuffer(f.read(n_cols * 2), dtype=">i2")  # big-endian int16
             data[i, :] = row_data.astype(np.float64)
 
     # Compute actual bounds
@@ -578,8 +571,7 @@ def load_gebco(
     """
     if version not in GEBCO_PARAMETERS:
         raise ValueError(
-            f"Unknown GEBCO version: {version}. "
-            f"Valid versions: {list(GEBCO_PARAMETERS.keys())}"
+            f"Unknown GEBCO version: {version}. " f"Valid versions: {list(GEBCO_PARAMETERS.keys())}"
         )
 
     return _load_gebco_cached(version, lat_min, lat_max, lon_min, lon_max)

pytcl/terrain/visibility.py

@@ -184,10 +184,7 @@ def line_of_sight(
     # Compute distances from observer
     dlat = sample_lats - obs_lat
     dlon = sample_lons - obs_lon
-    a = (
-        np.sin(dlat / 2) ** 2
-        + np.cos(obs_lat) * np.cos(sample_lats) * np.sin(dlon / 2) ** 2
-    )
+    a = np.sin(dlat / 2) ** 2 + np.cos(obs_lat) * np.cos(sample_lats) * np.sin(dlon / 2) ** 2
     c = 2 * np.arctan2(np.sqrt(a), np.sqrt(1 - a))
     distances = earth_radius * c
 
@@ -250,9 +247,7 @@ def line_of_sight(
         else:
             grazing_angle = -np.pi / 2
 
-    return LOSResult(
-        visible, grazing_angle, obstacle_distance, obstacle_elevation, min_clearance
-    )
+    return LOSResult(visible, grazing_angle, obstacle_distance, obstacle_elevation, min_clearance)
 
 
 def viewshed(

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

@@ -294,10 +294,7 @@ def n_scan_prune(
                     hyp_tracks_at_cutoff.add(track_id)
 
         # Keep if tracks match (or if no tracks at cutoff)
-        if (
-            hyp_tracks_at_cutoff == best_tracks_at_cutoff
-            or len(best_tracks_at_cutoff) == 0
-        ):
+        if hyp_tracks_at_cutoff == best_tracks_at_cutoff or len(best_tracks_at_cutoff) == 0:
             pruned.append(hyp)
 
     # Renormalize probabilities
@@ -515,9 +512,7 @@ class HypothesisTree:
         new_hypotheses = []
 
         for hyp in self.hypotheses:
-            for assoc_idx, (assoc, likelihood) in enumerate(
-                zip(associations, likelihoods)
-            ):
+            for assoc_idx, (assoc, likelihood) in enumerate(zip(associations, likelihoods)):
                 # Compute new hypothesis probability
                 new_prob = hyp.probability * likelihood
 

pytcl/trackers/mht.py

@@ -223,9 +223,7 @@ class MHTTracker:
         predicted_tracks = self._predict_tracks(current_tracks, F, Q)
 
         # Compute gating and likelihoods
-        gated, likelihood_matrix = self._compute_gating_and_likelihoods(
-            predicted_tracks, Z
-        )
+        gated, likelihood_matrix = self._compute_gating_and_likelihoods(predicted_tracks, Z)
 
         # Generate associations for each hypothesis
         track_id_list = list(predicted_tracks.keys())
@@ -270,9 +268,7 @@ class MHTTracker:
 
         # Update tracks based on associations
         new_tracks_per_assoc: Dict[int, List[MHTTrack]] = {}
-        updated_tracks: Dict[int, Dict[int, MHTTrack]] = (
-            {}
-        )  # assoc_idx -> track_id -> track
+        updated_tracks: Dict[int, Dict[int, MHTTrack]] = {}  # assoc_idx -> track_id -> track
 
         for assoc_idx, assoc in enumerate(associations):
             updated_tracks[assoc_idx] = {}
@@ -295,9 +291,7 @@ class MHTTracker:
                 updated_tracks[assoc_idx][track_id] = upd_track
 
             # Handle unassigned measurements -> new tracks
-            assigned_meas = set(
-                meas_idx for meas_idx in assoc.values() if meas_idx >= 0
-            )
+            assigned_meas = set(meas_idx for meas_idx in assoc.values() if meas_idx >= 0)
             for j in range(n_meas):
                 if j not in assigned_meas:
                     new_track = self._initiate_track(Z[j], j)
@@ -408,9 +402,7 @@ class MHTTracker:
 
         # Clutter and new track terms for unassigned measurements
         n_unassigned = len(Z) - len(used_meas)
-        likelihood *= (
-            self.config.clutter_density + self.config.new_track_weight
-        ) ** n_unassigned
+        likelihood *= (self.config.clutter_density + self.config.new_track_weight) ** n_unassigned
 
         return likelihood
 
@@ -534,9 +526,7 @@ class MHTTracker:
         new_hypotheses = []
 
         for hyp in self.hypothesis_tree.hypotheses:
-            for assoc_idx, (assoc, likelihood) in enumerate(
-                zip(associations, likelihoods)
-            ):
+            for assoc_idx, (assoc, likelihood) in enumerate(zip(associations, likelihoods)):
                 # Compute new hypothesis probability
                 new_prob = hyp.probability * likelihood
 

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):