nrl-tracker 1.6.0__py3-none-any.whl → 1.7.1__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- {nrl_tracker-1.6.0.dist-info → nrl_tracker-1.7.1.dist-info}/METADATA +14 -10
- {nrl_tracker-1.6.0.dist-info → nrl_tracker-1.7.1.dist-info}/RECORD +75 -68
- pytcl/__init__.py +2 -2
- pytcl/assignment_algorithms/__init__.py +28 -0
- pytcl/assignment_algorithms/gating.py +10 -10
- pytcl/assignment_algorithms/jpda.py +40 -40
- pytcl/assignment_algorithms/nd_assignment.py +379 -0
- pytcl/assignment_algorithms/network_flow.py +371 -0
- pytcl/assignment_algorithms/three_dimensional/assignment.py +3 -3
- pytcl/astronomical/__init__.py +35 -0
- pytcl/astronomical/ephemerides.py +14 -11
- pytcl/astronomical/reference_frames.py +110 -4
- pytcl/astronomical/relativity.py +6 -5
- pytcl/astronomical/special_orbits.py +532 -0
- pytcl/atmosphere/__init__.py +11 -0
- pytcl/atmosphere/nrlmsise00.py +809 -0
- pytcl/clustering/dbscan.py +2 -2
- pytcl/clustering/gaussian_mixture.py +3 -3
- pytcl/clustering/hierarchical.py +15 -15
- pytcl/clustering/kmeans.py +4 -4
- pytcl/containers/base.py +3 -3
- pytcl/containers/cluster_set.py +12 -2
- pytcl/containers/covertree.py +5 -3
- pytcl/containers/rtree.py +1 -1
- pytcl/containers/vptree.py +4 -2
- pytcl/coordinate_systems/conversions/geodetic.py +272 -5
- pytcl/coordinate_systems/jacobians/jacobians.py +2 -2
- pytcl/coordinate_systems/projections/projections.py +2 -2
- pytcl/coordinate_systems/rotations/rotations.py +10 -6
- pytcl/core/validation.py +3 -3
- pytcl/dynamic_estimation/__init__.py +26 -0
- pytcl/dynamic_estimation/gaussian_sum_filter.py +434 -0
- pytcl/dynamic_estimation/imm.py +14 -14
- pytcl/dynamic_estimation/kalman/__init__.py +12 -0
- pytcl/dynamic_estimation/kalman/constrained.py +382 -0
- pytcl/dynamic_estimation/kalman/extended.py +8 -8
- pytcl/dynamic_estimation/kalman/h_infinity.py +2 -2
- pytcl/dynamic_estimation/kalman/square_root.py +8 -2
- pytcl/dynamic_estimation/kalman/sr_ukf.py +3 -3
- pytcl/dynamic_estimation/kalman/ud_filter.py +11 -5
- pytcl/dynamic_estimation/kalman/unscented.py +8 -6
- pytcl/dynamic_estimation/particle_filters/bootstrap.py +15 -15
- pytcl/dynamic_estimation/rbpf.py +589 -0
- pytcl/gravity/spherical_harmonics.py +3 -3
- pytcl/gravity/tides.py +6 -6
- pytcl/logging_config.py +3 -3
- pytcl/magnetism/emm.py +10 -3
- pytcl/magnetism/wmm.py +4 -4
- pytcl/mathematical_functions/combinatorics/combinatorics.py +5 -5
- pytcl/mathematical_functions/geometry/geometry.py +5 -5
- pytcl/mathematical_functions/numerical_integration/quadrature.py +6 -6
- pytcl/mathematical_functions/signal_processing/detection.py +24 -24
- pytcl/mathematical_functions/signal_processing/filters.py +14 -14
- pytcl/mathematical_functions/signal_processing/matched_filter.py +12 -12
- pytcl/mathematical_functions/special_functions/bessel.py +15 -3
- pytcl/mathematical_functions/special_functions/debye.py +5 -1
- pytcl/mathematical_functions/special_functions/error_functions.py +3 -1
- pytcl/mathematical_functions/special_functions/gamma_functions.py +4 -4
- pytcl/mathematical_functions/special_functions/hypergeometric.py +6 -4
- pytcl/mathematical_functions/transforms/fourier.py +8 -8
- pytcl/mathematical_functions/transforms/stft.py +12 -12
- pytcl/mathematical_functions/transforms/wavelets.py +9 -9
- pytcl/navigation/geodesy.py +3 -3
- pytcl/navigation/great_circle.py +5 -5
- pytcl/plotting/coordinates.py +7 -7
- pytcl/plotting/tracks.py +2 -2
- pytcl/static_estimation/maximum_likelihood.py +16 -14
- pytcl/static_estimation/robust.py +5 -5
- pytcl/terrain/loaders.py +5 -5
- pytcl/trackers/hypothesis.py +1 -1
- pytcl/trackers/mht.py +9 -9
- pytcl/trackers/multi_target.py +1 -1
- {nrl_tracker-1.6.0.dist-info → nrl_tracker-1.7.1.dist-info}/LICENSE +0 -0
- {nrl_tracker-1.6.0.dist-info → nrl_tracker-1.7.1.dist-info}/WHEEL +0 -0
- {nrl_tracker-1.6.0.dist-info → nrl_tracker-1.7.1.dist-info}/top_level.txt +0 -0
pytcl/clustering/dbscan.py
CHANGED
|
@@ -12,7 +12,7 @@ References
|
|
|
12
12
|
with Noise," KDD 1996.
|
|
13
13
|
"""
|
|
14
14
|
|
|
15
|
-
from typing import List, NamedTuple, Set
|
|
15
|
+
from typing import Any, List, NamedTuple, Set
|
|
16
16
|
|
|
17
17
|
import numpy as np
|
|
18
18
|
from numba import njit
|
|
@@ -42,7 +42,7 @@ class DBSCANResult(NamedTuple):
|
|
|
42
42
|
|
|
43
43
|
|
|
44
44
|
@njit(cache=True)
|
|
45
|
-
def _compute_distance_matrix(X: np.ndarray) -> np.ndarray:
|
|
45
|
+
def _compute_distance_matrix(X: np.ndarray[Any, Any]) -> np.ndarray[Any, Any]:
|
|
46
46
|
"""Compute pairwise Euclidean distance matrix (JIT-compiled)."""
|
|
47
47
|
n = X.shape[0]
|
|
48
48
|
dist = np.zeros((n, n), dtype=np.float64)
|
|
@@ -674,9 +674,9 @@ class GaussianMixture:
|
|
|
674
674
|
|
|
675
675
|
def _gaussian_pdf(
|
|
676
676
|
self,
|
|
677
|
-
x: NDArray,
|
|
678
|
-
mean: NDArray,
|
|
679
|
-
cov: NDArray,
|
|
677
|
+
x: NDArray[np.floating],
|
|
678
|
+
mean: NDArray[np.floating],
|
|
679
|
+
cov: NDArray[np.floating],
|
|
680
680
|
) -> float:
|
|
681
681
|
"""Evaluate single Gaussian PDF."""
|
|
682
682
|
n = len(x)
|
pytcl/clustering/hierarchical.py
CHANGED
|
@@ -12,7 +12,7 @@ References
|
|
|
12
12
|
"""
|
|
13
13
|
|
|
14
14
|
from enum import Enum
|
|
15
|
-
from typing import List, Literal, NamedTuple, Optional
|
|
15
|
+
from typing import Any, List, Literal, NamedTuple, Optional
|
|
16
16
|
|
|
17
17
|
import numpy as np
|
|
18
18
|
from numba import njit
|
|
@@ -72,7 +72,7 @@ class HierarchicalResult(NamedTuple):
|
|
|
72
72
|
|
|
73
73
|
|
|
74
74
|
@njit(cache=True)
|
|
75
|
-
def _compute_distance_matrix_jit(X: np.ndarray) -> np.ndarray:
|
|
75
|
+
def _compute_distance_matrix_jit(X: np.ndarray[Any, Any]) -> np.ndarray[Any, Any]:
|
|
76
76
|
"""JIT-compiled pairwise Euclidean distance computation."""
|
|
77
77
|
n = X.shape[0]
|
|
78
78
|
n_features = X.shape[1]
|
|
@@ -112,43 +112,43 @@ def compute_distance_matrix(
|
|
|
112
112
|
|
|
113
113
|
|
|
114
114
|
def _single_linkage(
|
|
115
|
-
dist_i: NDArray,
|
|
116
|
-
dist_j: NDArray,
|
|
115
|
+
dist_i: NDArray[Any],
|
|
116
|
+
dist_j: NDArray[Any],
|
|
117
117
|
size_i: int,
|
|
118
118
|
size_j: int,
|
|
119
|
-
) -> NDArray:
|
|
119
|
+
) -> NDArray[Any]:
|
|
120
120
|
"""Single linkage: minimum of distances."""
|
|
121
121
|
return np.minimum(dist_i, dist_j)
|
|
122
122
|
|
|
123
123
|
|
|
124
124
|
def _complete_linkage(
|
|
125
|
-
dist_i: NDArray,
|
|
126
|
-
dist_j: NDArray,
|
|
125
|
+
dist_i: NDArray[Any],
|
|
126
|
+
dist_j: NDArray[Any],
|
|
127
127
|
size_i: int,
|
|
128
128
|
size_j: int,
|
|
129
|
-
) -> NDArray:
|
|
129
|
+
) -> NDArray[Any]:
|
|
130
130
|
"""Complete linkage: maximum of distances."""
|
|
131
131
|
return np.maximum(dist_i, dist_j)
|
|
132
132
|
|
|
133
133
|
|
|
134
134
|
def _average_linkage(
|
|
135
|
-
dist_i: NDArray,
|
|
136
|
-
dist_j: NDArray,
|
|
135
|
+
dist_i: NDArray[Any],
|
|
136
|
+
dist_j: NDArray[Any],
|
|
137
137
|
size_i: int,
|
|
138
138
|
size_j: int,
|
|
139
|
-
) -> NDArray:
|
|
139
|
+
) -> NDArray[Any]:
|
|
140
140
|
"""Average linkage: weighted average of distances."""
|
|
141
141
|
return (size_i * dist_i + size_j * dist_j) / (size_i + size_j)
|
|
142
142
|
|
|
143
143
|
|
|
144
144
|
def _ward_linkage(
|
|
145
|
-
dist_i: NDArray,
|
|
146
|
-
dist_j: NDArray,
|
|
145
|
+
dist_i: NDArray[Any],
|
|
146
|
+
dist_j: NDArray[Any],
|
|
147
147
|
size_i: int,
|
|
148
148
|
size_j: int,
|
|
149
|
-
size_k: NDArray,
|
|
149
|
+
size_k: NDArray[Any],
|
|
150
150
|
dist_ij: float,
|
|
151
|
-
) -> NDArray:
|
|
151
|
+
) -> NDArray[Any]:
|
|
152
152
|
"""Ward's linkage: minimum variance merge."""
|
|
153
153
|
total = size_i + size_j + size_k
|
|
154
154
|
return np.sqrt(
|
pytcl/clustering/kmeans.py
CHANGED
|
@@ -10,7 +10,7 @@ References
|
|
|
10
10
|
Careful Seeding," SODA 2007.
|
|
11
11
|
"""
|
|
12
12
|
|
|
13
|
-
from typing import Literal, NamedTuple, Optional, Union
|
|
13
|
+
from typing import Any, Literal, NamedTuple, Optional, Union
|
|
14
14
|
|
|
15
15
|
import numpy as np
|
|
16
16
|
from numpy.typing import ArrayLike, NDArray
|
|
@@ -305,7 +305,7 @@ def _kmeans_single(
|
|
|
305
305
|
|
|
306
306
|
# Handle empty clusters: keep old center
|
|
307
307
|
for k in range(n_clusters):
|
|
308
|
-
if np.all(new_centers[k] == 0) and np.any(labels == k)
|
|
308
|
+
if np.all(new_centers[k] == 0) and not np.any(labels == k):
|
|
309
309
|
new_centers[k] = centers[k]
|
|
310
310
|
|
|
311
311
|
# Check convergence
|
|
@@ -336,8 +336,8 @@ def _kmeans_single(
|
|
|
336
336
|
def kmeans_elbow(
|
|
337
337
|
X: ArrayLike,
|
|
338
338
|
k_range: Optional[range] = None,
|
|
339
|
-
**kwargs,
|
|
340
|
-
) -> dict:
|
|
339
|
+
**kwargs: Any,
|
|
340
|
+
) -> dict[str, Any]:
|
|
341
341
|
"""
|
|
342
342
|
Compute K-means for a range of k values for elbow method.
|
|
343
343
|
|
pytcl/containers/base.py
CHANGED
|
@@ -8,7 +8,7 @@ Cover trees.
|
|
|
8
8
|
|
|
9
9
|
import logging
|
|
10
10
|
from abc import ABC, abstractmethod
|
|
11
|
-
from typing import Callable, List, NamedTuple, Optional
|
|
11
|
+
from typing import Any, Callable, List, NamedTuple, Optional
|
|
12
12
|
|
|
13
13
|
import numpy as np
|
|
14
14
|
from numpy.typing import ArrayLike, NDArray
|
|
@@ -155,7 +155,7 @@ class MetricSpatialIndex(BaseSpatialIndex):
|
|
|
155
155
|
def __init__(
|
|
156
156
|
self,
|
|
157
157
|
data: ArrayLike,
|
|
158
|
-
metric: Optional[Callable[[NDArray, NDArray], float]] = None,
|
|
158
|
+
metric: Optional[Callable[[NDArray[Any], NDArray[Any]], float]] = None,
|
|
159
159
|
):
|
|
160
160
|
super().__init__(data)
|
|
161
161
|
|
|
@@ -165,7 +165,7 @@ class MetricSpatialIndex(BaseSpatialIndex):
|
|
|
165
165
|
self.metric = metric
|
|
166
166
|
|
|
167
167
|
@staticmethod
|
|
168
|
-
def _euclidean_distance(x: NDArray, y: NDArray) -> float:
|
|
168
|
+
def _euclidean_distance(x: NDArray[Any], y: NDArray[Any]) -> float:
|
|
169
169
|
"""Default Euclidean distance metric."""
|
|
170
170
|
return float(np.sqrt(np.sum((x - y) ** 2)))
|
|
171
171
|
|
pytcl/containers/cluster_set.py
CHANGED
|
@@ -7,7 +7,17 @@ that move together (formations, convoys, etc.).
|
|
|
7
7
|
|
|
8
8
|
from __future__ import annotations
|
|
9
9
|
|
|
10
|
-
from typing import
|
|
10
|
+
from typing import (
|
|
11
|
+
Any,
|
|
12
|
+
Dict,
|
|
13
|
+
Iterable,
|
|
14
|
+
Iterator,
|
|
15
|
+
List,
|
|
16
|
+
NamedTuple,
|
|
17
|
+
Optional,
|
|
18
|
+
Tuple,
|
|
19
|
+
Union,
|
|
20
|
+
)
|
|
11
21
|
|
|
12
22
|
import numpy as np
|
|
13
23
|
from numpy.typing import ArrayLike, NDArray
|
|
@@ -303,7 +313,7 @@ class ClusterSet:
|
|
|
303
313
|
cls,
|
|
304
314
|
tracks: TrackList,
|
|
305
315
|
method: str = "dbscan",
|
|
306
|
-
**kwargs,
|
|
316
|
+
**kwargs: Any,
|
|
307
317
|
) -> ClusterSet:
|
|
308
318
|
"""
|
|
309
319
|
Create a ClusterSet by clustering tracks.
|
pytcl/containers/covertree.py
CHANGED
|
@@ -12,7 +12,7 @@ References
|
|
|
12
12
|
"""
|
|
13
13
|
|
|
14
14
|
import logging
|
|
15
|
-
from typing import Callable, List, NamedTuple, Optional, Set, Tuple
|
|
15
|
+
from typing import Any, Callable, List, NamedTuple, Optional, Set, Tuple
|
|
16
16
|
|
|
17
17
|
import numpy as np
|
|
18
18
|
from numpy.typing import ArrayLike, NDArray
|
|
@@ -109,7 +109,9 @@ class CoverTree(MetricSpatialIndex):
|
|
|
109
109
|
def __init__(
|
|
110
110
|
self,
|
|
111
111
|
data: ArrayLike,
|
|
112
|
-
metric: Optional[
|
|
112
|
+
metric: Optional[
|
|
113
|
+
Callable[[np.ndarray[Any, Any], np.ndarray[Any, Any]], float]
|
|
114
|
+
] = None,
|
|
113
115
|
base: float = 2.0,
|
|
114
116
|
):
|
|
115
117
|
super().__init__(data, metric)
|
|
@@ -141,7 +143,7 @@ class CoverTree(MetricSpatialIndex):
|
|
|
141
143
|
self._distance_cache[key] = self.metric(self.data[i], self.data[j])
|
|
142
144
|
return self._distance_cache[key]
|
|
143
145
|
|
|
144
|
-
def _distance_to_point(self, idx: int, query: NDArray) -> float:
|
|
146
|
+
def _distance_to_point(self, idx: int, query: NDArray[np.floating]) -> float:
|
|
145
147
|
"""Distance from data point to query point."""
|
|
146
148
|
return self.metric(self.data[idx], query)
|
|
147
149
|
|
pytcl/containers/rtree.py
CHANGED
|
@@ -654,7 +654,7 @@ class RTree:
|
|
|
654
654
|
query = np.asarray(query_point, dtype=np.float64)
|
|
655
655
|
neighbors: List[Tuple[float, int]] = []
|
|
656
656
|
|
|
657
|
-
def min_dist_to_box(point: NDArray, bbox: BoundingBox) -> float:
|
|
657
|
+
def min_dist_to_box(point: NDArray[np.floating], bbox: BoundingBox) -> float:
|
|
658
658
|
"""Minimum distance from point to bounding box."""
|
|
659
659
|
clamped = np.clip(point, bbox.min_coords, bbox.max_coords)
|
|
660
660
|
return float(np.sqrt(np.sum((point - clamped) ** 2)))
|
pytcl/containers/vptree.py
CHANGED
|
@@ -12,7 +12,7 @@ References
|
|
|
12
12
|
"""
|
|
13
13
|
|
|
14
14
|
import logging
|
|
15
|
-
from typing import Callable, List, NamedTuple, Optional, Tuple
|
|
15
|
+
from typing import Any, Callable, List, NamedTuple, Optional, Tuple
|
|
16
16
|
|
|
17
17
|
import numpy as np
|
|
18
18
|
from numpy.typing import ArrayLike, NDArray
|
|
@@ -105,7 +105,9 @@ class VPTree(MetricSpatialIndex):
|
|
|
105
105
|
def __init__(
|
|
106
106
|
self,
|
|
107
107
|
data: ArrayLike,
|
|
108
|
-
metric: Optional[
|
|
108
|
+
metric: Optional[
|
|
109
|
+
Callable[[np.ndarray[Any, Any], np.ndarray[Any, Any]], float]
|
|
110
|
+
] = None,
|
|
109
111
|
):
|
|
110
112
|
super().__init__(data, metric)
|
|
111
113
|
|
|
@@ -165,11 +165,13 @@ def ecef2geodetic(
|
|
|
165
165
|
N = a / np.sqrt(1 - e2 * sin_lat**2)
|
|
166
166
|
|
|
167
167
|
# Altitude
|
|
168
|
-
|
|
169
|
-
|
|
170
|
-
|
|
171
|
-
|
|
172
|
-
|
|
168
|
+
# Use cos_lat when available, otherwise use sin_lat with guard against division by zero
|
|
169
|
+
with np.errstate(divide="ignore", invalid="ignore"):
|
|
170
|
+
alt = np.where(
|
|
171
|
+
np.abs(cos_lat) > 1e-10,
|
|
172
|
+
p / cos_lat - N,
|
|
173
|
+
np.abs(z) / np.abs(sin_lat) - N * (1 - e2),
|
|
174
|
+
)
|
|
173
175
|
else:
|
|
174
176
|
# Direct/closed-form method (simplified Vermeille)
|
|
175
177
|
zp = np.abs(z)
|
|
@@ -525,6 +527,267 @@ def ned2enu(ned: ArrayLike) -> NDArray[np.floating]:
|
|
|
525
527
|
return np.array([ned[1], ned[0], -ned[2]], dtype=np.float64)
|
|
526
528
|
|
|
527
529
|
|
|
530
|
+
def geodetic2sez(
|
|
531
|
+
lat: ArrayLike,
|
|
532
|
+
lon: ArrayLike,
|
|
533
|
+
alt: ArrayLike,
|
|
534
|
+
lat_ref: float,
|
|
535
|
+
lon_ref: float,
|
|
536
|
+
alt_ref: float,
|
|
537
|
+
a: float = WGS84.a,
|
|
538
|
+
f: float = WGS84.f,
|
|
539
|
+
) -> NDArray[np.floating]:
|
|
540
|
+
"""
|
|
541
|
+
Convert geodetic coordinates to local SEZ (South-East-Zenith) coordinates.
|
|
542
|
+
|
|
543
|
+
SEZ is a horizon-relative coordinate frame where:
|
|
544
|
+
- S (South) points in the southward direction
|
|
545
|
+
- E (East) points in the eastward direction
|
|
546
|
+
- Z (Zenith) points upward (away from Earth center)
|
|
547
|
+
|
|
548
|
+
Parameters
|
|
549
|
+
----------
|
|
550
|
+
lat : array_like
|
|
551
|
+
Geodetic latitude in radians.
|
|
552
|
+
lon : array_like
|
|
553
|
+
Geodetic longitude in radians.
|
|
554
|
+
alt : array_like
|
|
555
|
+
Altitude in meters.
|
|
556
|
+
lat_ref : float
|
|
557
|
+
Reference point latitude in radians.
|
|
558
|
+
lon_ref : float
|
|
559
|
+
Reference point longitude in radians.
|
|
560
|
+
alt_ref : float
|
|
561
|
+
Reference point altitude in meters.
|
|
562
|
+
a : float, optional
|
|
563
|
+
Semi-major axis of the reference ellipsoid.
|
|
564
|
+
f : float, optional
|
|
565
|
+
Flattening of the reference ellipsoid.
|
|
566
|
+
|
|
567
|
+
Returns
|
|
568
|
+
-------
|
|
569
|
+
sez : ndarray
|
|
570
|
+
Local SEZ coordinates [south, east, zenith] in meters.
|
|
571
|
+
|
|
572
|
+
See Also
|
|
573
|
+
--------
|
|
574
|
+
sez2geodetic : Inverse conversion.
|
|
575
|
+
ecef2sez : ECEF to SEZ conversion.
|
|
576
|
+
|
|
577
|
+
Notes
|
|
578
|
+
-----
|
|
579
|
+
SEZ is equivalent to NED when azimuth is measured from south.
|
|
580
|
+
Conversion: SEZ = [S, E, Z] = [NED[0], NED[1], -NED[2]]
|
|
581
|
+
|
|
582
|
+
Examples
|
|
583
|
+
--------
|
|
584
|
+
>>> sez = geodetic2sez(lat, lon, alt, lat_ref, lon_ref, alt_ref)
|
|
585
|
+
"""
|
|
586
|
+
# Convert both to ECEF
|
|
587
|
+
ecef = geodetic2ecef(lat, lon, alt, a, f)
|
|
588
|
+
ecef_ref = geodetic2ecef(lat_ref, lon_ref, alt_ref, a, f)
|
|
589
|
+
|
|
590
|
+
# Get SEZ from ECEF difference
|
|
591
|
+
return ecef2sez(ecef, lat_ref, lon_ref, ecef_ref)
|
|
592
|
+
|
|
593
|
+
|
|
594
|
+
def ecef2sez(
|
|
595
|
+
ecef: ArrayLike,
|
|
596
|
+
lat_ref: float,
|
|
597
|
+
lon_ref: float,
|
|
598
|
+
ecef_ref: Optional[ArrayLike] = None,
|
|
599
|
+
) -> NDArray[np.floating]:
|
|
600
|
+
"""
|
|
601
|
+
Convert ECEF coordinates to local SEZ coordinates.
|
|
602
|
+
|
|
603
|
+
Parameters
|
|
604
|
+
----------
|
|
605
|
+
ecef : array_like
|
|
606
|
+
ECEF coordinates [X, Y, Z] in meters, shape (3,) or (3, N).
|
|
607
|
+
lat_ref : float
|
|
608
|
+
Reference point latitude in radians.
|
|
609
|
+
lon_ref : float
|
|
610
|
+
Reference point longitude in radians.
|
|
611
|
+
ecef_ref : array_like, optional
|
|
612
|
+
Reference ECEF position. If None, the reference point is
|
|
613
|
+
at (lat_ref, lon_ref) with zero altitude.
|
|
614
|
+
|
|
615
|
+
Returns
|
|
616
|
+
-------
|
|
617
|
+
sez : ndarray
|
|
618
|
+
SEZ coordinates [south, east, zenith] in meters.
|
|
619
|
+
|
|
620
|
+
See Also
|
|
621
|
+
--------
|
|
622
|
+
sez2ecef : Inverse conversion.
|
|
623
|
+
"""
|
|
624
|
+
ecef = np.asarray(ecef, dtype=np.float64)
|
|
625
|
+
|
|
626
|
+
if ecef_ref is None:
|
|
627
|
+
ecef_ref = geodetic2ecef(lat_ref, lon_ref, 0.0)
|
|
628
|
+
else:
|
|
629
|
+
ecef_ref = np.asarray(ecef_ref, dtype=np.float64)
|
|
630
|
+
|
|
631
|
+
# Relative position in ECEF
|
|
632
|
+
if ecef.ndim == 1:
|
|
633
|
+
delta_ecef = ecef - ecef_ref
|
|
634
|
+
else:
|
|
635
|
+
if ecef.shape[0] != 3:
|
|
636
|
+
ecef = ecef.T
|
|
637
|
+
if ecef_ref.ndim == 1:
|
|
638
|
+
delta_ecef = ecef - ecef_ref[:, np.newaxis]
|
|
639
|
+
else:
|
|
640
|
+
delta_ecef = ecef - ecef_ref[:, np.newaxis]
|
|
641
|
+
|
|
642
|
+
# Rotation matrix from ECEF to SEZ
|
|
643
|
+
sin_lat = np.sin(lat_ref)
|
|
644
|
+
cos_lat = np.cos(lat_ref)
|
|
645
|
+
sin_lon = np.sin(lon_ref)
|
|
646
|
+
cos_lon = np.cos(lon_ref)
|
|
647
|
+
|
|
648
|
+
# SEZ rotation matrix (transforms ECEF delta to SEZ)
|
|
649
|
+
# S = -sin(lat)*cos(lon)*dX - sin(lat)*sin(lon)*dY + cos(lat)*dZ
|
|
650
|
+
# E = -sin(lon)*dX + cos(lon)*dY
|
|
651
|
+
# Z = cos(lat)*cos(lon)*dX + cos(lat)*sin(lon)*dY + sin(lat)*dZ
|
|
652
|
+
|
|
653
|
+
if delta_ecef.ndim == 1:
|
|
654
|
+
s = (
|
|
655
|
+
-sin_lat * cos_lon * delta_ecef[0]
|
|
656
|
+
- sin_lat * sin_lon * delta_ecef[1]
|
|
657
|
+
+ cos_lat * delta_ecef[2]
|
|
658
|
+
)
|
|
659
|
+
e = -sin_lon * delta_ecef[0] + cos_lon * delta_ecef[1]
|
|
660
|
+
z = (
|
|
661
|
+
cos_lat * cos_lon * delta_ecef[0]
|
|
662
|
+
+ cos_lat * sin_lon * delta_ecef[1]
|
|
663
|
+
+ sin_lat * delta_ecef[2]
|
|
664
|
+
)
|
|
665
|
+
return np.array([s, e, z], dtype=np.float64)
|
|
666
|
+
else:
|
|
667
|
+
s = (
|
|
668
|
+
-sin_lat * cos_lon * delta_ecef[0, :]
|
|
669
|
+
- sin_lat * sin_lon * delta_ecef[1, :]
|
|
670
|
+
+ cos_lat * delta_ecef[2, :]
|
|
671
|
+
)
|
|
672
|
+
e = -sin_lon * delta_ecef[0, :] + cos_lon * delta_ecef[1, :]
|
|
673
|
+
z = (
|
|
674
|
+
cos_lat * cos_lon * delta_ecef[0, :]
|
|
675
|
+
+ cos_lat * sin_lon * delta_ecef[1, :]
|
|
676
|
+
+ sin_lat * delta_ecef[2, :]
|
|
677
|
+
)
|
|
678
|
+
return np.array([s, e, z], dtype=np.float64)
|
|
679
|
+
|
|
680
|
+
|
|
681
|
+
def sez2ecef(
|
|
682
|
+
sez: ArrayLike,
|
|
683
|
+
lat_ref: float,
|
|
684
|
+
lon_ref: float,
|
|
685
|
+
ecef_ref: Optional[ArrayLike] = None,
|
|
686
|
+
) -> NDArray[np.floating]:
|
|
687
|
+
"""
|
|
688
|
+
Convert local SEZ coordinates to ECEF coordinates.
|
|
689
|
+
|
|
690
|
+
Parameters
|
|
691
|
+
----------
|
|
692
|
+
sez : array_like
|
|
693
|
+
SEZ coordinates [south, east, zenith] in meters, shape (3,) or (3, N).
|
|
694
|
+
lat_ref : float
|
|
695
|
+
Reference point latitude in radians.
|
|
696
|
+
lon_ref : float
|
|
697
|
+
Reference point longitude in radians.
|
|
698
|
+
ecef_ref : array_like, optional
|
|
699
|
+
Reference ECEF position. If None, the reference point is
|
|
700
|
+
at (lat_ref, lon_ref) with zero altitude.
|
|
701
|
+
|
|
702
|
+
Returns
|
|
703
|
+
-------
|
|
704
|
+
ecef : ndarray
|
|
705
|
+
ECEF coordinates [X, Y, Z] in meters.
|
|
706
|
+
|
|
707
|
+
See Also
|
|
708
|
+
--------
|
|
709
|
+
ecef2sez : Forward conversion.
|
|
710
|
+
"""
|
|
711
|
+
sez = np.asarray(sez, dtype=np.float64)
|
|
712
|
+
|
|
713
|
+
if ecef_ref is None:
|
|
714
|
+
ecef_ref = geodetic2ecef(lat_ref, lon_ref, 0.0)
|
|
715
|
+
else:
|
|
716
|
+
ecef_ref = np.asarray(ecef_ref, dtype=np.float64)
|
|
717
|
+
|
|
718
|
+
# Rotation matrix from SEZ to ECEF (transpose of ECEF to SEZ)
|
|
719
|
+
sin_lat = np.sin(lat_ref)
|
|
720
|
+
cos_lat = np.cos(lat_ref)
|
|
721
|
+
sin_lon = np.sin(lon_ref)
|
|
722
|
+
cos_lon = np.cos(lon_ref)
|
|
723
|
+
|
|
724
|
+
# Inverse rotation: ECEF = ECEF_ref + R_inv @ SEZ
|
|
725
|
+
if sez.ndim == 1:
|
|
726
|
+
dX = -sin_lat * cos_lon * sez[0] - sin_lon * sez[1] + cos_lat * cos_lon * sez[2]
|
|
727
|
+
dY = -sin_lat * sin_lon * sez[0] + cos_lon * sez[1] + cos_lat * sin_lon * sez[2]
|
|
728
|
+
dZ = cos_lat * sez[0] + sin_lat * sez[2]
|
|
729
|
+
return ecef_ref + np.array([dX, dY, dZ], dtype=np.float64)
|
|
730
|
+
else:
|
|
731
|
+
if sez.shape[0] != 3:
|
|
732
|
+
sez = sez.T
|
|
733
|
+
dX = (
|
|
734
|
+
-sin_lat * cos_lon * sez[0, :]
|
|
735
|
+
- sin_lon * sez[1, :]
|
|
736
|
+
+ cos_lat * cos_lon * sez[2, :]
|
|
737
|
+
)
|
|
738
|
+
dY = (
|
|
739
|
+
-sin_lat * sin_lon * sez[0, :]
|
|
740
|
+
+ cos_lon * sez[1, :]
|
|
741
|
+
+ cos_lat * sin_lon * sez[2, :]
|
|
742
|
+
)
|
|
743
|
+
dZ = cos_lat * sez[0, :] + sin_lat * sez[2, :]
|
|
744
|
+
return ecef_ref[:, np.newaxis] + np.array([dX, dY, dZ], dtype=np.float64)
|
|
745
|
+
|
|
746
|
+
|
|
747
|
+
def sez2geodetic(
|
|
748
|
+
sez: ArrayLike,
|
|
749
|
+
lat_ref: float,
|
|
750
|
+
lon_ref: float,
|
|
751
|
+
alt_ref: float,
|
|
752
|
+
a: float = WGS84.a,
|
|
753
|
+
f: float = WGS84.f,
|
|
754
|
+
) -> Tuple[NDArray[np.floating], NDArray[np.floating], NDArray[np.floating]]:
|
|
755
|
+
"""
|
|
756
|
+
Convert local SEZ coordinates to geodetic coordinates.
|
|
757
|
+
|
|
758
|
+
Parameters
|
|
759
|
+
----------
|
|
760
|
+
sez : array_like
|
|
761
|
+
SEZ coordinates [south, east, zenith] in meters.
|
|
762
|
+
lat_ref : float
|
|
763
|
+
Reference point latitude in radians.
|
|
764
|
+
lon_ref : float
|
|
765
|
+
Reference point longitude in radians.
|
|
766
|
+
alt_ref : float
|
|
767
|
+
Reference point altitude in meters.
|
|
768
|
+
a : float, optional
|
|
769
|
+
Semi-major axis.
|
|
770
|
+
f : float, optional
|
|
771
|
+
Flattening.
|
|
772
|
+
|
|
773
|
+
Returns
|
|
774
|
+
-------
|
|
775
|
+
lat : ndarray
|
|
776
|
+
Geodetic latitude in radians.
|
|
777
|
+
lon : ndarray
|
|
778
|
+
Geodetic longitude in radians.
|
|
779
|
+
alt : ndarray
|
|
780
|
+
Altitude in meters.
|
|
781
|
+
|
|
782
|
+
See Also
|
|
783
|
+
--------
|
|
784
|
+
geodetic2sez : Forward conversion.
|
|
785
|
+
"""
|
|
786
|
+
ecef_ref = geodetic2ecef(lat_ref, lon_ref, alt_ref, a, f)
|
|
787
|
+
ecef = sez2ecef(sez, lat_ref, lon_ref, ecef_ref)
|
|
788
|
+
return ecef2geodetic(ecef, a, f)
|
|
789
|
+
|
|
790
|
+
|
|
528
791
|
def geocentric_radius(
|
|
529
792
|
lat: ArrayLike,
|
|
530
793
|
a: float = WGS84.a,
|
|
@@ -625,6 +888,10 @@ __all__ = [
|
|
|
625
888
|
"ned2ecef",
|
|
626
889
|
"enu2ned",
|
|
627
890
|
"ned2enu",
|
|
891
|
+
"geodetic2sez",
|
|
892
|
+
"ecef2sez",
|
|
893
|
+
"sez2ecef",
|
|
894
|
+
"sez2geodetic",
|
|
628
895
|
"geocentric_radius",
|
|
629
896
|
"prime_vertical_radius",
|
|
630
897
|
"meridional_radius",
|
|
@@ -6,7 +6,7 @@ coordinate transformations, essential for error propagation in tracking
|
|
|
6
6
|
filters (e.g., converting measurement covariances between coordinate systems).
|
|
7
7
|
"""
|
|
8
8
|
|
|
9
|
-
from typing import Literal
|
|
9
|
+
from typing import Callable, Literal
|
|
10
10
|
|
|
11
11
|
import numpy as np
|
|
12
12
|
from numpy.typing import ArrayLike, NDArray
|
|
@@ -431,7 +431,7 @@ def cross_covariance_transform(
|
|
|
431
431
|
|
|
432
432
|
|
|
433
433
|
def numerical_jacobian(
|
|
434
|
-
func,
|
|
434
|
+
func: Callable[[ArrayLike], ArrayLike],
|
|
435
435
|
x: ArrayLike,
|
|
436
436
|
dx: float = 1e-7,
|
|
437
437
|
) -> NDArray[np.floating]:
|
|
@@ -25,7 +25,7 @@ References
|
|
|
25
25
|
nanometers." Journal of Geodesy 85.8 (2011): 475-485.
|
|
26
26
|
"""
|
|
27
27
|
|
|
28
|
-
from typing import NamedTuple, Optional, Tuple
|
|
28
|
+
from typing import Any, NamedTuple, Optional, Tuple
|
|
29
29
|
|
|
30
30
|
import numpy as np
|
|
31
31
|
from numpy.typing import NDArray
|
|
@@ -1253,7 +1253,7 @@ def geodetic2utm_batch(
|
|
|
1253
1253
|
lats: NDArray[np.floating],
|
|
1254
1254
|
lons: NDArray[np.floating],
|
|
1255
1255
|
zone: Optional[int] = None,
|
|
1256
|
-
) ->
|
|
1256
|
+
) -> tuple[NDArray[np.floating], NDArray[np.floating], NDArray[np.intp], NDArray[Any]]:
|
|
1257
1257
|
"""
|
|
1258
1258
|
Batch convert geodetic coordinates to UTM.
|
|
1259
1259
|
|
|
@@ -6,7 +6,7 @@ representations including rotation matrices, quaternions, Euler angles,
|
|
|
6
6
|
axis-angle, and Rodrigues parameters.
|
|
7
7
|
"""
|
|
8
8
|
|
|
9
|
-
from typing import Tuple
|
|
9
|
+
from typing import Any, Tuple
|
|
10
10
|
|
|
11
11
|
import numpy as np
|
|
12
12
|
from numba import njit
|
|
@@ -14,7 +14,7 @@ from numpy.typing import ArrayLike, NDArray
|
|
|
14
14
|
|
|
15
15
|
|
|
16
16
|
@njit(cache=True, fastmath=True)
|
|
17
|
-
def _rotx_inplace(angle: float, R: np.ndarray) -> None:
|
|
17
|
+
def _rotx_inplace(angle: float, R: np.ndarray[Any, Any]) -> None:
|
|
18
18
|
"""JIT-compiled rotation about x-axis (fills existing matrix)."""
|
|
19
19
|
c = np.cos(angle)
|
|
20
20
|
s = np.sin(angle)
|
|
@@ -30,7 +30,7 @@ def _rotx_inplace(angle: float, R: np.ndarray) -> None:
|
|
|
30
30
|
|
|
31
31
|
|
|
32
32
|
@njit(cache=True, fastmath=True)
|
|
33
|
-
def _roty_inplace(angle: float, R: np.ndarray) -> None:
|
|
33
|
+
def _roty_inplace(angle: float, R: np.ndarray[Any, Any]) -> None:
|
|
34
34
|
"""JIT-compiled rotation about y-axis (fills existing matrix)."""
|
|
35
35
|
c = np.cos(angle)
|
|
36
36
|
s = np.sin(angle)
|
|
@@ -46,7 +46,7 @@ def _roty_inplace(angle: float, R: np.ndarray) -> None:
|
|
|
46
46
|
|
|
47
47
|
|
|
48
48
|
@njit(cache=True, fastmath=True)
|
|
49
|
-
def _rotz_inplace(angle: float, R: np.ndarray) -> None:
|
|
49
|
+
def _rotz_inplace(angle: float, R: np.ndarray[Any, Any]) -> None:
|
|
50
50
|
"""JIT-compiled rotation about z-axis (fills existing matrix)."""
|
|
51
51
|
c = np.cos(angle)
|
|
52
52
|
s = np.sin(angle)
|
|
@@ -62,7 +62,9 @@ def _rotz_inplace(angle: float, R: np.ndarray) -> None:
|
|
|
62
62
|
|
|
63
63
|
|
|
64
64
|
@njit(cache=True, fastmath=True)
|
|
65
|
-
def _euler_zyx_to_rotmat(
|
|
65
|
+
def _euler_zyx_to_rotmat(
|
|
66
|
+
yaw: float, pitch: float, roll: float, R: np.ndarray[Any, Any]
|
|
67
|
+
) -> None:
|
|
66
68
|
"""JIT-compiled ZYX Euler angles to rotation matrix."""
|
|
67
69
|
cy = np.cos(yaw)
|
|
68
70
|
sy = np.sin(yaw)
|
|
@@ -84,7 +86,9 @@ def _euler_zyx_to_rotmat(yaw: float, pitch: float, roll: float, R: np.ndarray) -
|
|
|
84
86
|
|
|
85
87
|
|
|
86
88
|
@njit(cache=True, fastmath=True)
|
|
87
|
-
def _matmul_3x3(
|
|
89
|
+
def _matmul_3x3(
|
|
90
|
+
A: np.ndarray[Any, Any], B: np.ndarray[Any, Any], C: np.ndarray[Any, Any]
|
|
91
|
+
) -> None:
|
|
88
92
|
"""JIT-compiled 3x3 matrix multiplication C = A @ B."""
|
|
89
93
|
for i in range(3):
|
|
90
94
|
for j in range(3):
|
pytcl/core/validation.py
CHANGED
|
@@ -28,7 +28,7 @@ def validate_array(
|
|
|
28
28
|
arr: ArrayLike,
|
|
29
29
|
name: str = "array",
|
|
30
30
|
*,
|
|
31
|
-
dtype: type | np.dtype | None = None,
|
|
31
|
+
dtype: type | np.dtype[Any] | None = None,
|
|
32
32
|
ndim: int | tuple[int, ...] | None = None,
|
|
33
33
|
shape: tuple[int | None, ...] | None = None,
|
|
34
34
|
min_ndim: int | None = None,
|
|
@@ -415,7 +415,7 @@ def validate_same_shape(*arrays: ArrayLike, names: Sequence[str] | None = None)
|
|
|
415
415
|
def validated_array_input(
|
|
416
416
|
param_name: str,
|
|
417
417
|
*,
|
|
418
|
-
dtype: type | np.dtype | None = None,
|
|
418
|
+
dtype: type | np.dtype[Any] | None = None,
|
|
419
419
|
ndim: int | tuple[int, ...] | None = None,
|
|
420
420
|
shape: tuple[int | None, ...] | None = None,
|
|
421
421
|
finite: bool = False,
|
|
@@ -516,7 +516,7 @@ class ArraySpec:
|
|
|
516
516
|
def __init__(
|
|
517
517
|
self,
|
|
518
518
|
*,
|
|
519
|
-
dtype: type | np.dtype | None = None,
|
|
519
|
+
dtype: type | np.dtype[Any] | None = None,
|
|
520
520
|
ndim: int | tuple[int, ...] | None = None,
|
|
521
521
|
shape: tuple[int | None, ...] | None = None,
|
|
522
522
|
min_ndim: int | None = None,
|