nrl-tracker 0.21.5__py3-none-any.whl → 0.22.0__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-0.21.5.dist-info → nrl_tracker-0.22.0.dist-info}/METADATA +2 -2
- nrl_tracker-0.22.0.dist-info/RECORD +150 -0
- pytcl/__init__.py +9 -11
- pytcl/assignment_algorithms/__init__.py +32 -42
- pytcl/assignment_algorithms/data_association.py +9 -10
- pytcl/assignment_algorithms/gating.py +7 -5
- pytcl/assignment_algorithms/jpda.py +10 -14
- pytcl/assignment_algorithms/three_dimensional/__init__.py +6 -8
- pytcl/assignment_algorithms/three_dimensional/assignment.py +6 -2
- pytcl/assignment_algorithms/two_dimensional/__init__.py +9 -13
- pytcl/assignment_algorithms/two_dimensional/assignment.py +5 -2
- pytcl/assignment_algorithms/two_dimensional/kbest.py +9 -9
- pytcl/astronomical/__init__.py +130 -89
- pytcl/astronomical/ephemerides.py +524 -0
- pytcl/astronomical/lambert.py +6 -15
- pytcl/astronomical/orbital_mechanics.py +1 -3
- pytcl/astronomical/reference_frames.py +1 -3
- pytcl/astronomical/relativity.py +466 -0
- pytcl/astronomical/time_systems.py +2 -1
- pytcl/atmosphere/__init__.py +12 -14
- pytcl/atmosphere/models.py +5 -5
- pytcl/clustering/__init__.py +28 -36
- pytcl/clustering/dbscan.py +5 -2
- pytcl/clustering/gaussian_mixture.py +10 -10
- pytcl/clustering/hierarchical.py +7 -7
- pytcl/clustering/kmeans.py +7 -5
- pytcl/containers/__init__.py +29 -43
- pytcl/containers/cluster_set.py +13 -20
- pytcl/containers/covertree.py +8 -2
- pytcl/containers/kd_tree.py +6 -2
- pytcl/containers/measurement_set.py +11 -16
- pytcl/containers/rtree.py +8 -7
- pytcl/containers/track_list.py +13 -13
- pytcl/containers/vptree.py +7 -2
- pytcl/coordinate_systems/__init__.py +69 -74
- pytcl/coordinate_systems/conversions/__init__.py +20 -24
- pytcl/coordinate_systems/conversions/geodetic.py +7 -17
- pytcl/coordinate_systems/conversions/spherical.py +4 -2
- pytcl/coordinate_systems/jacobians/__init__.py +10 -12
- pytcl/coordinate_systems/jacobians/jacobians.py +2 -1
- pytcl/coordinate_systems/projections/__init__.py +27 -23
- pytcl/coordinate_systems/projections/projections.py +14 -39
- pytcl/coordinate_systems/rotations/__init__.py +20 -22
- pytcl/coordinate_systems/rotations/rotations.py +3 -4
- pytcl/core/__init__.py +16 -22
- pytcl/core/array_utils.py +7 -7
- pytcl/core/constants.py +1 -3
- pytcl/core/validation.py +13 -19
- pytcl/dynamic_estimation/__init__.py +77 -86
- pytcl/dynamic_estimation/imm.py +10 -15
- pytcl/dynamic_estimation/information_filter.py +8 -6
- pytcl/dynamic_estimation/kalman/__init__.py +40 -48
- pytcl/dynamic_estimation/kalman/extended.py +4 -5
- pytcl/dynamic_estimation/kalman/linear.py +7 -3
- pytcl/dynamic_estimation/kalman/square_root.py +7 -8
- pytcl/dynamic_estimation/kalman/unscented.py +8 -6
- pytcl/dynamic_estimation/particle_filters/__init__.py +12 -14
- pytcl/dynamic_estimation/particle_filters/bootstrap.py +8 -8
- pytcl/dynamic_estimation/smoothers.py +9 -10
- pytcl/dynamic_models/__init__.py +37 -41
- pytcl/dynamic_models/continuous_time/__init__.py +11 -11
- pytcl/dynamic_models/continuous_time/dynamics.py +4 -2
- pytcl/dynamic_models/discrete_time/__init__.py +11 -17
- pytcl/dynamic_models/process_noise/__init__.py +11 -17
- pytcl/dynamic_models/process_noise/polynomial.py +2 -6
- pytcl/gravity/__init__.py +55 -65
- pytcl/gravity/clenshaw.py +4 -7
- pytcl/gravity/egm.py +9 -6
- pytcl/gravity/models.py +1 -3
- pytcl/gravity/spherical_harmonics.py +6 -11
- pytcl/gravity/tides.py +9 -17
- pytcl/magnetism/__init__.py +26 -36
- pytcl/magnetism/emm.py +7 -13
- pytcl/magnetism/igrf.py +5 -6
- pytcl/magnetism/wmm.py +4 -10
- pytcl/mathematical_functions/__init__.py +69 -87
- pytcl/mathematical_functions/basic_matrix/__init__.py +25 -19
- pytcl/mathematical_functions/basic_matrix/decompositions.py +6 -5
- pytcl/mathematical_functions/basic_matrix/special_matrices.py +2 -1
- pytcl/mathematical_functions/combinatorics/__init__.py +18 -14
- pytcl/mathematical_functions/combinatorics/combinatorics.py +5 -4
- pytcl/mathematical_functions/geometry/__init__.py +15 -15
- pytcl/mathematical_functions/geometry/geometry.py +10 -15
- pytcl/mathematical_functions/interpolation/__init__.py +11 -13
- pytcl/mathematical_functions/interpolation/interpolation.py +8 -5
- pytcl/mathematical_functions/numerical_integration/__init__.py +16 -10
- pytcl/mathematical_functions/numerical_integration/quadrature.py +6 -2
- pytcl/mathematical_functions/signal_processing/__init__.py +42 -30
- pytcl/mathematical_functions/signal_processing/detection.py +9 -9
- pytcl/mathematical_functions/signal_processing/filters.py +7 -8
- pytcl/mathematical_functions/signal_processing/matched_filter.py +8 -7
- pytcl/mathematical_functions/special_functions/__init__.py +75 -77
- pytcl/mathematical_functions/special_functions/bessel.py +2 -1
- pytcl/mathematical_functions/special_functions/debye.py +4 -2
- pytcl/mathematical_functions/special_functions/elliptic.py +3 -4
- pytcl/mathematical_functions/special_functions/error_functions.py +2 -1
- pytcl/mathematical_functions/special_functions/gamma_functions.py +3 -4
- pytcl/mathematical_functions/special_functions/hypergeometric.py +2 -1
- pytcl/mathematical_functions/special_functions/lambert_w.py +3 -4
- pytcl/mathematical_functions/special_functions/marcum_q.py +2 -1
- pytcl/mathematical_functions/statistics/__init__.py +27 -31
- pytcl/mathematical_functions/statistics/distributions.py +21 -40
- pytcl/mathematical_functions/statistics/estimators.py +3 -4
- pytcl/mathematical_functions/transforms/__init__.py +45 -51
- pytcl/mathematical_functions/transforms/fourier.py +5 -2
- pytcl/mathematical_functions/transforms/stft.py +8 -11
- pytcl/mathematical_functions/transforms/wavelets.py +13 -20
- pytcl/navigation/__init__.py +96 -102
- pytcl/navigation/geodesy.py +13 -33
- pytcl/navigation/great_circle.py +7 -13
- pytcl/navigation/ins.py +12 -16
- pytcl/navigation/ins_gnss.py +24 -37
- pytcl/navigation/rhumb.py +7 -12
- pytcl/performance_evaluation/__init__.py +21 -25
- pytcl/performance_evaluation/estimation_metrics.py +3 -1
- pytcl/performance_evaluation/track_metrics.py +4 -4
- pytcl/plotting/__init__.py +30 -38
- pytcl/plotting/coordinates.py +8 -18
- pytcl/plotting/ellipses.py +5 -2
- pytcl/plotting/metrics.py +5 -10
- pytcl/plotting/tracks.py +7 -12
- pytcl/static_estimation/__init__.py +37 -41
- pytcl/static_estimation/least_squares.py +5 -4
- pytcl/static_estimation/maximum_likelihood.py +8 -5
- pytcl/static_estimation/robust.py +5 -2
- pytcl/terrain/__init__.py +28 -34
- pytcl/terrain/dem.py +6 -9
- pytcl/terrain/loaders.py +9 -14
- pytcl/terrain/visibility.py +4 -8
- pytcl/trackers/__init__.py +17 -25
- pytcl/trackers/hypothesis.py +8 -8
- pytcl/trackers/mht.py +18 -24
- pytcl/trackers/multi_target.py +8 -6
- pytcl/trackers/single_target.py +5 -2
- nrl_tracker-0.21.5.dist-info/RECORD +0 -148
- {nrl_tracker-0.21.5.dist-info → nrl_tracker-0.22.0.dist-info}/LICENSE +0 -0
- {nrl_tracker-0.21.5.dist-info → nrl_tracker-0.22.0.dist-info}/WHEEL +0 -0
- {nrl_tracker-0.21.5.dist-info → nrl_tracker-0.22.0.dist-info}/top_level.txt +0 -0
|
@@ -10,18 +10,16 @@ This module provides:
|
|
|
10
10
|
- Covariance transformation utilities
|
|
11
11
|
"""
|
|
12
12
|
|
|
13
|
-
from pytcl.coordinate_systems.jacobians.jacobians import
|
|
14
|
-
|
|
15
|
-
|
|
16
|
-
|
|
17
|
-
|
|
18
|
-
|
|
19
|
-
|
|
20
|
-
|
|
21
|
-
|
|
22
|
-
|
|
23
|
-
spherical_jacobian_inv,
|
|
24
|
-
)
|
|
13
|
+
from pytcl.coordinate_systems.jacobians.jacobians import cross_covariance_transform
|
|
14
|
+
from pytcl.coordinate_systems.jacobians.jacobians import enu_jacobian
|
|
15
|
+
from pytcl.coordinate_systems.jacobians.jacobians import geodetic_jacobian
|
|
16
|
+
from pytcl.coordinate_systems.jacobians.jacobians import ned_jacobian
|
|
17
|
+
from pytcl.coordinate_systems.jacobians.jacobians import numerical_jacobian
|
|
18
|
+
from pytcl.coordinate_systems.jacobians.jacobians import polar_jacobian
|
|
19
|
+
from pytcl.coordinate_systems.jacobians.jacobians import polar_jacobian_inv
|
|
20
|
+
from pytcl.coordinate_systems.jacobians.jacobians import ruv_jacobian
|
|
21
|
+
from pytcl.coordinate_systems.jacobians.jacobians import spherical_jacobian
|
|
22
|
+
from pytcl.coordinate_systems.jacobians.jacobians import spherical_jacobian_inv
|
|
25
23
|
|
|
26
24
|
__all__ = [
|
|
27
25
|
"spherical_jacobian",
|
|
@@ -9,7 +9,8 @@ filters (e.g., converting measurement covariances between coordinate systems).
|
|
|
9
9
|
from typing import Literal
|
|
10
10
|
|
|
11
11
|
import numpy as np
|
|
12
|
-
from numpy.typing import ArrayLike
|
|
12
|
+
from numpy.typing import ArrayLike
|
|
13
|
+
from numpy.typing import NDArray
|
|
13
14
|
|
|
14
15
|
|
|
15
16
|
def spherical_jacobian(
|
|
@@ -26,32 +26,36 @@ Examples
|
|
|
26
26
|
... result.zone, result.hemisphere)
|
|
27
27
|
"""
|
|
28
28
|
|
|
29
|
-
from pytcl.coordinate_systems.projections.projections import (
|
|
30
|
-
WGS84_A,
|
|
31
|
-
|
|
32
|
-
|
|
33
|
-
|
|
34
|
-
|
|
35
|
-
|
|
36
|
-
|
|
37
|
-
|
|
38
|
-
|
|
29
|
+
from pytcl.coordinate_systems.projections.projections import (
|
|
30
|
+
WGS84_A, # Constants; Result types; Azimuthal Equidistant; UTM; Lambert Conformal Conic; Mercator; Stereographic; Transverse Mercator
|
|
31
|
+
)
|
|
32
|
+
from pytcl.coordinate_systems.projections.projections import WGS84_B
|
|
33
|
+
from pytcl.coordinate_systems.projections.projections import WGS84_E
|
|
34
|
+
from pytcl.coordinate_systems.projections.projections import WGS84_E2
|
|
35
|
+
from pytcl.coordinate_systems.projections.projections import WGS84_EP2
|
|
36
|
+
from pytcl.coordinate_systems.projections.projections import WGS84_F
|
|
37
|
+
from pytcl.coordinate_systems.projections.projections import ProjectionResult
|
|
38
|
+
from pytcl.coordinate_systems.projections.projections import UTMResult
|
|
39
|
+
from pytcl.coordinate_systems.projections.projections import azimuthal_equidistant
|
|
40
|
+
from pytcl.coordinate_systems.projections.projections import (
|
|
39
41
|
azimuthal_equidistant_inverse,
|
|
40
|
-
|
|
41
|
-
|
|
42
|
-
|
|
42
|
+
)
|
|
43
|
+
from pytcl.coordinate_systems.projections.projections import geodetic2utm
|
|
44
|
+
from pytcl.coordinate_systems.projections.projections import geodetic2utm_batch
|
|
45
|
+
from pytcl.coordinate_systems.projections.projections import lambert_conformal_conic
|
|
46
|
+
from pytcl.coordinate_systems.projections.projections import (
|
|
43
47
|
lambert_conformal_conic_inverse,
|
|
44
|
-
mercator,
|
|
45
|
-
mercator_inverse,
|
|
46
|
-
polar_stereographic,
|
|
47
|
-
stereographic,
|
|
48
|
-
stereographic_inverse,
|
|
49
|
-
transverse_mercator,
|
|
50
|
-
transverse_mercator_inverse,
|
|
51
|
-
utm2geodetic,
|
|
52
|
-
utm_central_meridian,
|
|
53
|
-
utm_zone,
|
|
54
48
|
)
|
|
49
|
+
from pytcl.coordinate_systems.projections.projections import mercator
|
|
50
|
+
from pytcl.coordinate_systems.projections.projections import mercator_inverse
|
|
51
|
+
from pytcl.coordinate_systems.projections.projections import polar_stereographic
|
|
52
|
+
from pytcl.coordinate_systems.projections.projections import stereographic
|
|
53
|
+
from pytcl.coordinate_systems.projections.projections import stereographic_inverse
|
|
54
|
+
from pytcl.coordinate_systems.projections.projections import transverse_mercator
|
|
55
|
+
from pytcl.coordinate_systems.projections.projections import transverse_mercator_inverse
|
|
56
|
+
from pytcl.coordinate_systems.projections.projections import utm2geodetic
|
|
57
|
+
from pytcl.coordinate_systems.projections.projections import utm_central_meridian
|
|
58
|
+
from pytcl.coordinate_systems.projections.projections import utm_zone
|
|
55
59
|
|
|
56
60
|
__all__ = [
|
|
57
61
|
# Constants
|
|
@@ -25,7 +25,9 @@ References
|
|
|
25
25
|
nanometers." Journal of Geodesy 85.8 (2011): 475-485.
|
|
26
26
|
"""
|
|
27
27
|
|
|
28
|
-
from typing import NamedTuple
|
|
28
|
+
from typing import NamedTuple
|
|
29
|
+
from typing import Optional
|
|
30
|
+
from typing import Tuple
|
|
29
31
|
|
|
30
32
|
import numpy as np
|
|
31
33
|
from numpy.typing import NDArray
|
|
@@ -140,9 +142,7 @@ def mercator(
|
|
|
140
142
|
|
|
141
143
|
# Northing using isometric latitude
|
|
142
144
|
sin_lat = np.sin(lat)
|
|
143
|
-
y = a * np.log(
|
|
144
|
-
np.tan(np.pi / 4 + lat / 2) * ((1 - e * sin_lat) / (1 + e * sin_lat)) ** (e / 2)
|
|
145
|
-
)
|
|
145
|
+
y = a * np.log(np.tan(np.pi / 4 + lat / 2) * ((1 - e * sin_lat) / (1 + e * sin_lat)) ** (e / 2))
|
|
146
146
|
|
|
147
147
|
# Scale factor
|
|
148
148
|
cos_lat = np.cos(lat)
|
|
@@ -203,9 +203,7 @@ def mercator_inverse(
|
|
|
203
203
|
|
|
204
204
|
for _ in range(max_iter):
|
|
205
205
|
sin_lat = np.sin(lat)
|
|
206
|
-
lat_new = np.pi / 2 - 2 * np.arctan(
|
|
207
|
-
t * ((1 - e * sin_lat) / (1 + e * sin_lat)) ** (e / 2)
|
|
208
|
-
)
|
|
206
|
+
lat_new = np.pi / 2 - 2 * np.arctan(t * ((1 - e * sin_lat) / (1 + e * sin_lat)) ** (e / 2))
|
|
209
207
|
if abs(lat_new - lat) < tol:
|
|
210
208
|
break
|
|
211
209
|
lat = lat_new
|
|
@@ -583,9 +581,7 @@ def geodetic2utm(lat: float, lon: float, zone: Optional[int] = None) -> UTMResul
|
|
|
583
581
|
northing = result.y + 10000000.0
|
|
584
582
|
hemisphere = "S"
|
|
585
583
|
|
|
586
|
-
return UTMResult(
|
|
587
|
-
easting, northing, zone, hemisphere, result.scale, result.convergence
|
|
588
|
-
)
|
|
584
|
+
return UTMResult(easting, northing, zone, hemisphere, result.scale, result.convergence)
|
|
589
585
|
|
|
590
586
|
|
|
591
587
|
def utm2geodetic(
|
|
@@ -696,8 +692,7 @@ def stereographic(
|
|
|
696
692
|
return (
|
|
697
693
|
2
|
|
698
694
|
* np.arctan(
|
|
699
|
-
np.tan(np.pi / 4 + phi / 2)
|
|
700
|
-
* ((1 - e * sin_phi) / (1 + e * sin_phi)) ** (e / 2)
|
|
695
|
+
np.tan(np.pi / 4 + phi / 2) * ((1 - e * sin_phi) / (1 + e * sin_phi)) ** (e / 2)
|
|
701
696
|
)
|
|
702
697
|
- np.pi / 2
|
|
703
698
|
)
|
|
@@ -785,8 +780,7 @@ def stereographic_inverse(
|
|
|
785
780
|
chi0 = (
|
|
786
781
|
2
|
|
787
782
|
* np.arctan(
|
|
788
|
-
np.tan(np.pi / 4 + lat0 / 2)
|
|
789
|
-
* ((1 - e * sin_lat0) / (1 + e * sin_lat0)) ** (e / 2)
|
|
783
|
+
np.tan(np.pi / 4 + lat0 / 2) * ((1 - e * sin_lat0) / (1 + e * sin_lat0)) ** (e / 2)
|
|
790
784
|
)
|
|
791
785
|
- np.pi / 2
|
|
792
786
|
)
|
|
@@ -821,8 +815,7 @@ def stereographic_inverse(
|
|
|
821
815
|
lat_new = (
|
|
822
816
|
2
|
|
823
817
|
* np.arctan(
|
|
824
|
-
np.tan(np.pi / 4 + chi / 2)
|
|
825
|
-
* ((1 + e * sin_lat) / (1 - e * sin_lat)) ** (e / 2)
|
|
818
|
+
np.tan(np.pi / 4 + chi / 2) * ((1 + e * sin_lat) / (1 - e * sin_lat)) ** (e / 2)
|
|
826
819
|
)
|
|
827
820
|
- np.pi / 2
|
|
828
821
|
)
|
|
@@ -954,9 +947,7 @@ def lambert_conformal_conic(
|
|
|
954
947
|
|
|
955
948
|
def compute_t(phi: float) -> float:
|
|
956
949
|
sin_phi = np.sin(phi)
|
|
957
|
-
return np.tan(np.pi / 4 - phi / 2) / (
|
|
958
|
-
((1 - e * sin_phi) / (1 + e * sin_phi)) ** (e / 2)
|
|
959
|
-
)
|
|
950
|
+
return np.tan(np.pi / 4 - phi / 2) / (((1 - e * sin_phi) / (1 + e * sin_phi)) ** (e / 2))
|
|
960
951
|
|
|
961
952
|
m1 = compute_m(lat1)
|
|
962
953
|
m2 = compute_m(lat2)
|
|
@@ -1045,9 +1036,7 @@ def lambert_conformal_conic_inverse(
|
|
|
1045
1036
|
|
|
1046
1037
|
def compute_t(phi: float) -> float:
|
|
1047
1038
|
sin_phi = np.sin(phi)
|
|
1048
|
-
return np.tan(np.pi / 4 - phi / 2) / (
|
|
1049
|
-
((1 - e * sin_phi) / (1 + e * sin_phi)) ** (e / 2)
|
|
1050
|
-
)
|
|
1039
|
+
return np.tan(np.pi / 4 - phi / 2) / (((1 - e * sin_phi) / (1 + e * sin_phi)) ** (e / 2))
|
|
1051
1040
|
|
|
1052
1041
|
m1 = compute_m(lat1)
|
|
1053
1042
|
m2 = compute_m(lat2)
|
|
@@ -1075,9 +1064,7 @@ def lambert_conformal_conic_inverse(
|
|
|
1075
1064
|
lat = np.pi / 2 - 2 * np.arctan(t)
|
|
1076
1065
|
for _ in range(max_iter):
|
|
1077
1066
|
sin_lat = np.sin(lat)
|
|
1078
|
-
lat_new = np.pi / 2 - 2 * np.arctan(
|
|
1079
|
-
t * ((1 - e * sin_lat) / (1 + e * sin_lat)) ** (e / 2)
|
|
1080
|
-
)
|
|
1067
|
+
lat_new = np.pi / 2 - 2 * np.arctan(t * ((1 - e * sin_lat) / (1 + e * sin_lat)) ** (e / 2))
|
|
1081
1068
|
if abs(lat_new - lat) < tol:
|
|
1082
1069
|
break
|
|
1083
1070
|
lat = lat_new
|
|
@@ -1141,13 +1128,7 @@ def azimuthal_equidistant(
|
|
|
1141
1128
|
"""
|
|
1142
1129
|
# Use spherical approximation with authalic radius
|
|
1143
1130
|
R = a * np.sqrt(
|
|
1144
|
-
(
|
|
1145
|
-
1
|
|
1146
|
-
+ (1 - e2)
|
|
1147
|
-
/ (2 * np.sqrt(1 - e2))
|
|
1148
|
-
* np.log((1 + np.sqrt(1 - e2)) / np.sqrt(e2))
|
|
1149
|
-
)
|
|
1150
|
-
/ 2
|
|
1131
|
+
(1 + (1 - e2) / (2 * np.sqrt(1 - e2)) * np.log((1 + np.sqrt(1 - e2)) / np.sqrt(e2))) / 2
|
|
1151
1132
|
)
|
|
1152
1133
|
|
|
1153
1134
|
sin_lat = np.sin(lat)
|
|
@@ -1217,13 +1198,7 @@ def azimuthal_equidistant_inverse(
|
|
|
1217
1198
|
(latitude, longitude) in radians.
|
|
1218
1199
|
"""
|
|
1219
1200
|
R = a * np.sqrt(
|
|
1220
|
-
(
|
|
1221
|
-
1
|
|
1222
|
-
+ (1 - e2)
|
|
1223
|
-
/ (2 * np.sqrt(1 - e2))
|
|
1224
|
-
* np.log((1 + np.sqrt(1 - e2)) / np.sqrt(e2))
|
|
1225
|
-
)
|
|
1226
|
-
/ 2
|
|
1201
|
+
(1 + (1 - e2) / (2 * np.sqrt(1 - e2)) * np.log((1 + np.sqrt(1 - e2)) / np.sqrt(e2))) / 2
|
|
1227
1202
|
)
|
|
1228
1203
|
|
|
1229
1204
|
rho = np.sqrt(x**2 + y**2)
|
|
@@ -9,28 +9,26 @@ This module provides:
|
|
|
9
9
|
- Rotation interpolation (SLERP)
|
|
10
10
|
"""
|
|
11
11
|
|
|
12
|
-
from pytcl.coordinate_systems.rotations.rotations import
|
|
13
|
-
|
|
14
|
-
|
|
15
|
-
|
|
16
|
-
|
|
17
|
-
|
|
18
|
-
|
|
19
|
-
|
|
20
|
-
|
|
21
|
-
|
|
22
|
-
|
|
23
|
-
|
|
24
|
-
|
|
25
|
-
|
|
26
|
-
|
|
27
|
-
|
|
28
|
-
|
|
29
|
-
|
|
30
|
-
|
|
31
|
-
|
|
32
|
-
slerp,
|
|
33
|
-
)
|
|
12
|
+
from pytcl.coordinate_systems.rotations.rotations import axisangle2rotmat
|
|
13
|
+
from pytcl.coordinate_systems.rotations.rotations import dcm_rate
|
|
14
|
+
from pytcl.coordinate_systems.rotations.rotations import euler2quat
|
|
15
|
+
from pytcl.coordinate_systems.rotations.rotations import euler2rotmat
|
|
16
|
+
from pytcl.coordinate_systems.rotations.rotations import is_rotation_matrix
|
|
17
|
+
from pytcl.coordinate_systems.rotations.rotations import quat2euler
|
|
18
|
+
from pytcl.coordinate_systems.rotations.rotations import quat2rotmat
|
|
19
|
+
from pytcl.coordinate_systems.rotations.rotations import quat_conjugate
|
|
20
|
+
from pytcl.coordinate_systems.rotations.rotations import quat_inverse
|
|
21
|
+
from pytcl.coordinate_systems.rotations.rotations import quat_multiply
|
|
22
|
+
from pytcl.coordinate_systems.rotations.rotations import quat_rotate
|
|
23
|
+
from pytcl.coordinate_systems.rotations.rotations import rodrigues2rotmat
|
|
24
|
+
from pytcl.coordinate_systems.rotations.rotations import rotmat2axisangle
|
|
25
|
+
from pytcl.coordinate_systems.rotations.rotations import rotmat2euler
|
|
26
|
+
from pytcl.coordinate_systems.rotations.rotations import rotmat2quat
|
|
27
|
+
from pytcl.coordinate_systems.rotations.rotations import rotmat2rodrigues
|
|
28
|
+
from pytcl.coordinate_systems.rotations.rotations import rotx
|
|
29
|
+
from pytcl.coordinate_systems.rotations.rotations import roty
|
|
30
|
+
from pytcl.coordinate_systems.rotations.rotations import rotz
|
|
31
|
+
from pytcl.coordinate_systems.rotations.rotations import slerp
|
|
34
32
|
|
|
35
33
|
__all__ = [
|
|
36
34
|
"rotx",
|
|
@@ -10,7 +10,8 @@ from typing import Tuple
|
|
|
10
10
|
|
|
11
11
|
import numpy as np
|
|
12
12
|
from numba import njit
|
|
13
|
-
from numpy.typing import ArrayLike
|
|
13
|
+
from numpy.typing import ArrayLike
|
|
14
|
+
from numpy.typing import NDArray
|
|
14
15
|
|
|
15
16
|
|
|
16
17
|
@njit(cache=True, fastmath=True)
|
|
@@ -353,9 +354,7 @@ def rotmat2axisangle(
|
|
|
353
354
|
return axis / np.linalg.norm(axis), float(angle)
|
|
354
355
|
|
|
355
356
|
# General case
|
|
356
|
-
axis = np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]]) / (
|
|
357
|
-
2 * np.sin(angle)
|
|
358
|
-
)
|
|
357
|
+
axis = np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]]) / (2 * np.sin(angle))
|
|
359
358
|
|
|
360
359
|
return axis, float(angle)
|
|
361
360
|
|
pytcl/core/__init__.py
CHANGED
|
@@ -7,28 +7,22 @@ This module provides foundational functionality used throughout the library:
|
|
|
7
7
|
- Array manipulation helpers compatible with MATLAB conventions
|
|
8
8
|
"""
|
|
9
9
|
|
|
10
|
-
from pytcl.core.array_utils import
|
|
11
|
-
|
|
12
|
-
|
|
13
|
-
|
|
14
|
-
|
|
15
|
-
|
|
16
|
-
|
|
17
|
-
from pytcl.core.constants import
|
|
18
|
-
|
|
19
|
-
|
|
20
|
-
|
|
21
|
-
|
|
22
|
-
|
|
23
|
-
|
|
24
|
-
|
|
25
|
-
|
|
26
|
-
from pytcl.core.validation import (
|
|
27
|
-
ensure_2d,
|
|
28
|
-
ensure_column_vector,
|
|
29
|
-
ensure_row_vector,
|
|
30
|
-
validate_array,
|
|
31
|
-
)
|
|
10
|
+
from pytcl.core.array_utils import column_vector
|
|
11
|
+
from pytcl.core.array_utils import row_vector
|
|
12
|
+
from pytcl.core.array_utils import wrap_to_2pi
|
|
13
|
+
from pytcl.core.array_utils import wrap_to_pi
|
|
14
|
+
from pytcl.core.array_utils import wrap_to_range
|
|
15
|
+
from pytcl.core.constants import EARTH_FLATTENING
|
|
16
|
+
from pytcl.core.constants import EARTH_ROTATION_RATE
|
|
17
|
+
from pytcl.core.constants import EARTH_SEMI_MAJOR_AXIS
|
|
18
|
+
from pytcl.core.constants import GRAVITATIONAL_CONSTANT
|
|
19
|
+
from pytcl.core.constants import SPEED_OF_LIGHT
|
|
20
|
+
from pytcl.core.constants import WGS84
|
|
21
|
+
from pytcl.core.constants import PhysicalConstants
|
|
22
|
+
from pytcl.core.validation import ensure_2d
|
|
23
|
+
from pytcl.core.validation import ensure_column_vector
|
|
24
|
+
from pytcl.core.validation import ensure_row_vector
|
|
25
|
+
from pytcl.core.validation import validate_array
|
|
32
26
|
|
|
33
27
|
__all__ = [
|
|
34
28
|
# Constants
|
pytcl/core/array_utils.py
CHANGED
|
@@ -7,12 +7,15 @@ making it easier to port algorithms while maintaining Pythonic interfaces.
|
|
|
7
7
|
|
|
8
8
|
from __future__ import annotations
|
|
9
9
|
|
|
10
|
-
from typing import Any
|
|
10
|
+
from typing import Any
|
|
11
|
+
from typing import Literal
|
|
11
12
|
|
|
12
13
|
import numpy as np
|
|
13
|
-
from numpy.typing import ArrayLike
|
|
14
|
+
from numpy.typing import ArrayLike
|
|
15
|
+
from numpy.typing import NDArray
|
|
14
16
|
|
|
15
|
-
from pytcl.core.constants import PI
|
|
17
|
+
from pytcl.core.constants import PI
|
|
18
|
+
from pytcl.core.constants import TWO_PI
|
|
16
19
|
|
|
17
20
|
|
|
18
21
|
def wrap_to_pi(angle: ArrayLike) -> NDArray[np.floating[Any]]:
|
|
@@ -374,10 +377,7 @@ def normalize_vector(
|
|
|
374
377
|
v: ArrayLike,
|
|
375
378
|
axis: int | None = None,
|
|
376
379
|
return_norm: bool = False,
|
|
377
|
-
) ->
|
|
378
|
-
NDArray[np.floating[Any]]
|
|
379
|
-
| tuple[NDArray[np.floating[Any]], NDArray[np.floating[Any]]]
|
|
380
|
-
):
|
|
380
|
+
) -> NDArray[np.floating[Any]] | tuple[NDArray[np.floating[Any]], NDArray[np.floating[Any]]]:
|
|
381
381
|
"""
|
|
382
382
|
Normalize vector(s) to unit length.
|
|
383
383
|
|
pytcl/core/constants.py
CHANGED
|
@@ -71,9 +71,7 @@ EARTH_ECCENTRICITY_SQ: Final[float] = 2 * EARTH_FLATTENING - EARTH_FLATTENING**2
|
|
|
71
71
|
EARTH_ECCENTRICITY: Final[float] = math.sqrt(EARTH_ECCENTRICITY_SQ)
|
|
72
72
|
|
|
73
73
|
#: Second eccentricity squared
|
|
74
|
-
EARTH_ECCENTRICITY_PRIME_SQ: Final[float] = EARTH_ECCENTRICITY_SQ / (
|
|
75
|
-
1 - EARTH_ECCENTRICITY_SQ
|
|
76
|
-
)
|
|
74
|
+
EARTH_ECCENTRICITY_PRIME_SQ: Final[float] = EARTH_ECCENTRICITY_SQ / (1 - EARTH_ECCENTRICITY_SQ)
|
|
77
75
|
|
|
78
76
|
#: Earth rotation rate [rad/s] (IERS Conventions 2010)
|
|
79
77
|
EARTH_ROTATION_RATE: Final[float] = 7.292115e-5
|
pytcl/core/validation.py
CHANGED
|
@@ -9,10 +9,15 @@ messages when inputs don't meet requirements.
|
|
|
9
9
|
from __future__ import annotations
|
|
10
10
|
|
|
11
11
|
from functools import wraps
|
|
12
|
-
from typing import Any
|
|
12
|
+
from typing import Any
|
|
13
|
+
from typing import Callable
|
|
14
|
+
from typing import Literal
|
|
15
|
+
from typing import Sequence
|
|
16
|
+
from typing import TypeVar
|
|
13
17
|
|
|
14
18
|
import numpy as np
|
|
15
|
-
from numpy.typing import ArrayLike
|
|
19
|
+
from numpy.typing import ArrayLike
|
|
20
|
+
from numpy.typing import NDArray
|
|
16
21
|
|
|
17
22
|
# Type variable for generic function signatures
|
|
18
23
|
F = TypeVar("F", bound=Callable[..., Any])
|
|
@@ -108,9 +113,7 @@ def validate_array(
|
|
|
108
113
|
if ndim is not None:
|
|
109
114
|
valid_ndims = (ndim,) if isinstance(ndim, int) else ndim
|
|
110
115
|
if result.ndim not in valid_ndims:
|
|
111
|
-
raise ValidationError(
|
|
112
|
-
f"{name} must have {ndim} dimension(s), got {result.ndim}"
|
|
113
|
-
)
|
|
116
|
+
raise ValidationError(f"{name} must have {ndim} dimension(s), got {result.ndim}")
|
|
114
117
|
|
|
115
118
|
if min_ndim is not None and result.ndim < min_ndim:
|
|
116
119
|
raise ValidationError(
|
|
@@ -125,14 +128,10 @@ def validate_array(
|
|
|
125
128
|
# Check shape
|
|
126
129
|
if shape is not None:
|
|
127
130
|
if len(shape) != result.ndim:
|
|
128
|
-
raise ValidationError(
|
|
129
|
-
f"{name} must have {len(shape)} dimensions, got {result.ndim}"
|
|
130
|
-
)
|
|
131
|
+
raise ValidationError(f"{name} must have {len(shape)} dimensions, got {result.ndim}")
|
|
131
132
|
for i, (expected, actual) in enumerate(zip(shape, result.shape)):
|
|
132
133
|
if expected is not None and expected != actual:
|
|
133
|
-
raise ValidationError(
|
|
134
|
-
f"{name} dimension {i} must be {expected}, got {actual}"
|
|
135
|
-
)
|
|
134
|
+
raise ValidationError(f"{name} dimension {i} must be {expected}, got {actual}")
|
|
136
135
|
|
|
137
136
|
# Check finite
|
|
138
137
|
if finite and not np.all(np.isfinite(result)):
|
|
@@ -259,9 +258,7 @@ def ensure_row_vector(arr: ArrayLike, name: str = "vector") -> NDArray[Any]:
|
|
|
259
258
|
return result.reshape(1, -1)
|
|
260
259
|
elif result.ndim == 2:
|
|
261
260
|
if result.shape[0] != 1:
|
|
262
|
-
raise ValidationError(
|
|
263
|
-
f"{name} must be a row vector (1, n), got shape {result.shape}"
|
|
264
|
-
)
|
|
261
|
+
raise ValidationError(f"{name} must be a row vector (1, n), got shape {result.shape}")
|
|
265
262
|
return result
|
|
266
263
|
else:
|
|
267
264
|
raise ValidationError(f"{name} must be 1D or 2D, got {result.ndim}D")
|
|
@@ -374,8 +371,7 @@ def ensure_positive_definite(
|
|
|
374
371
|
|
|
375
372
|
if min_eigenvalue < threshold:
|
|
376
373
|
raise ValidationError(
|
|
377
|
-
f"{name} must be positive definite, "
|
|
378
|
-
f"minimum eigenvalue is {min_eigenvalue:.2e}"
|
|
374
|
+
f"{name} must be positive definite, " f"minimum eigenvalue is {min_eigenvalue:.2e}"
|
|
379
375
|
)
|
|
380
376
|
|
|
381
377
|
return result
|
|
@@ -407,9 +403,7 @@ def validate_same_shape(*arrays: ArrayLike, names: Sequence[str] | None = None)
|
|
|
407
403
|
|
|
408
404
|
if not all(s == shapes[0] for s in shapes):
|
|
409
405
|
shape_strs = [f"{name}: {shape}" for name, shape in zip(names, shapes)]
|
|
410
|
-
raise ValidationError(
|
|
411
|
-
f"Arrays must have the same shape. Got: {', '.join(shape_strs)}"
|
|
412
|
-
)
|
|
406
|
+
raise ValidationError(f"Arrays must have the same shape. Got: {', '.join(shape_strs)}")
|
|
413
407
|
|
|
414
408
|
|
|
415
409
|
def validated_array_input(
|
|
@@ -12,106 +12,97 @@ This module provides filtering and smoothing algorithms for state estimation:
|
|
|
12
12
|
"""
|
|
13
13
|
|
|
14
14
|
# Import submodules for easy access
|
|
15
|
-
from pytcl.dynamic_estimation import kalman
|
|
15
|
+
from pytcl.dynamic_estimation import kalman
|
|
16
|
+
from pytcl.dynamic_estimation import particle_filters
|
|
16
17
|
|
|
17
18
|
# IMM estimator
|
|
18
|
-
from pytcl.dynamic_estimation.imm import
|
|
19
|
-
|
|
20
|
-
|
|
21
|
-
|
|
22
|
-
|
|
23
|
-
|
|
24
|
-
|
|
25
|
-
imm_update,
|
|
26
|
-
)
|
|
19
|
+
from pytcl.dynamic_estimation.imm import IMMEstimator
|
|
20
|
+
from pytcl.dynamic_estimation.imm import IMMPrediction
|
|
21
|
+
from pytcl.dynamic_estimation.imm import IMMState
|
|
22
|
+
from pytcl.dynamic_estimation.imm import IMMUpdate
|
|
23
|
+
from pytcl.dynamic_estimation.imm import imm_predict
|
|
24
|
+
from pytcl.dynamic_estimation.imm import imm_predict_update
|
|
25
|
+
from pytcl.dynamic_estimation.imm import imm_update
|
|
27
26
|
|
|
28
27
|
# Information filter
|
|
29
|
-
from pytcl.dynamic_estimation.information_filter import
|
|
30
|
-
|
|
31
|
-
|
|
32
|
-
|
|
33
|
-
|
|
34
|
-
|
|
35
|
-
|
|
36
|
-
|
|
37
|
-
|
|
38
|
-
|
|
39
|
-
|
|
40
|
-
state_to_information,
|
|
41
|
-
)
|
|
28
|
+
from pytcl.dynamic_estimation.information_filter import InformationFilterResult
|
|
29
|
+
from pytcl.dynamic_estimation.information_filter import InformationState
|
|
30
|
+
from pytcl.dynamic_estimation.information_filter import SRIFResult
|
|
31
|
+
from pytcl.dynamic_estimation.information_filter import SRIFState
|
|
32
|
+
from pytcl.dynamic_estimation.information_filter import fuse_information
|
|
33
|
+
from pytcl.dynamic_estimation.information_filter import information_filter
|
|
34
|
+
from pytcl.dynamic_estimation.information_filter import information_to_state
|
|
35
|
+
from pytcl.dynamic_estimation.information_filter import srif_filter
|
|
36
|
+
from pytcl.dynamic_estimation.information_filter import srif_predict
|
|
37
|
+
from pytcl.dynamic_estimation.information_filter import srif_update
|
|
38
|
+
from pytcl.dynamic_estimation.information_filter import state_to_information
|
|
42
39
|
|
|
43
40
|
# Square-root Kalman filters
|
|
44
41
|
# Cubature Kalman filter
|
|
45
42
|
# Unscented Kalman filter
|
|
46
43
|
# Extended Kalman filter
|
|
47
44
|
# Linear Kalman filter
|
|
48
|
-
from pytcl.dynamic_estimation.kalman import
|
|
49
|
-
|
|
50
|
-
|
|
51
|
-
|
|
52
|
-
|
|
53
|
-
|
|
54
|
-
|
|
55
|
-
|
|
56
|
-
|
|
57
|
-
|
|
58
|
-
|
|
59
|
-
|
|
60
|
-
|
|
61
|
-
|
|
62
|
-
|
|
63
|
-
|
|
64
|
-
|
|
65
|
-
|
|
66
|
-
|
|
67
|
-
|
|
68
|
-
|
|
69
|
-
|
|
70
|
-
|
|
71
|
-
|
|
72
|
-
|
|
73
|
-
|
|
74
|
-
|
|
75
|
-
|
|
76
|
-
|
|
77
|
-
|
|
78
|
-
|
|
79
|
-
|
|
80
|
-
|
|
81
|
-
|
|
82
|
-
|
|
83
|
-
|
|
84
|
-
|
|
85
|
-
unscented_transform,
|
|
86
|
-
)
|
|
45
|
+
from pytcl.dynamic_estimation.kalman import KalmanPrediction
|
|
46
|
+
from pytcl.dynamic_estimation.kalman import KalmanState
|
|
47
|
+
from pytcl.dynamic_estimation.kalman import KalmanUpdate
|
|
48
|
+
from pytcl.dynamic_estimation.kalman import SigmaPoints
|
|
49
|
+
from pytcl.dynamic_estimation.kalman import SRKalmanPrediction
|
|
50
|
+
from pytcl.dynamic_estimation.kalman import SRKalmanState
|
|
51
|
+
from pytcl.dynamic_estimation.kalman import SRKalmanUpdate
|
|
52
|
+
from pytcl.dynamic_estimation.kalman import UDState
|
|
53
|
+
from pytcl.dynamic_estimation.kalman import ckf_predict
|
|
54
|
+
from pytcl.dynamic_estimation.kalman import ckf_spherical_cubature_points
|
|
55
|
+
from pytcl.dynamic_estimation.kalman import ckf_update
|
|
56
|
+
from pytcl.dynamic_estimation.kalman import ekf_predict
|
|
57
|
+
from pytcl.dynamic_estimation.kalman import ekf_predict_auto
|
|
58
|
+
from pytcl.dynamic_estimation.kalman import ekf_update
|
|
59
|
+
from pytcl.dynamic_estimation.kalman import ekf_update_auto
|
|
60
|
+
from pytcl.dynamic_estimation.kalman import information_filter_predict
|
|
61
|
+
from pytcl.dynamic_estimation.kalman import information_filter_update
|
|
62
|
+
from pytcl.dynamic_estimation.kalman import iterated_ekf_update
|
|
63
|
+
from pytcl.dynamic_estimation.kalman import kf_predict
|
|
64
|
+
from pytcl.dynamic_estimation.kalman import kf_predict_update
|
|
65
|
+
from pytcl.dynamic_estimation.kalman import kf_smooth
|
|
66
|
+
from pytcl.dynamic_estimation.kalman import kf_update
|
|
67
|
+
from pytcl.dynamic_estimation.kalman import numerical_jacobian
|
|
68
|
+
from pytcl.dynamic_estimation.kalman import sigma_points_julier
|
|
69
|
+
from pytcl.dynamic_estimation.kalman import sigma_points_merwe
|
|
70
|
+
from pytcl.dynamic_estimation.kalman import sr_ukf_predict
|
|
71
|
+
from pytcl.dynamic_estimation.kalman import sr_ukf_update
|
|
72
|
+
from pytcl.dynamic_estimation.kalman import srkf_predict
|
|
73
|
+
from pytcl.dynamic_estimation.kalman import srkf_predict_update
|
|
74
|
+
from pytcl.dynamic_estimation.kalman import srkf_update
|
|
75
|
+
from pytcl.dynamic_estimation.kalman import ud_factorize
|
|
76
|
+
from pytcl.dynamic_estimation.kalman import ud_predict
|
|
77
|
+
from pytcl.dynamic_estimation.kalman import ud_reconstruct
|
|
78
|
+
from pytcl.dynamic_estimation.kalman import ud_update
|
|
79
|
+
from pytcl.dynamic_estimation.kalman import ukf_predict
|
|
80
|
+
from pytcl.dynamic_estimation.kalman import ukf_update
|
|
81
|
+
from pytcl.dynamic_estimation.kalman import unscented_transform
|
|
87
82
|
|
|
88
83
|
# Particle filters
|
|
89
|
-
from pytcl.dynamic_estimation.particle_filters import
|
|
90
|
-
|
|
91
|
-
|
|
92
|
-
|
|
93
|
-
|
|
94
|
-
|
|
95
|
-
|
|
96
|
-
|
|
97
|
-
|
|
98
|
-
|
|
99
|
-
|
|
100
|
-
|
|
101
|
-
resample_systematic,
|
|
102
|
-
)
|
|
84
|
+
from pytcl.dynamic_estimation.particle_filters import ParticleState
|
|
85
|
+
from pytcl.dynamic_estimation.particle_filters import bootstrap_pf_predict
|
|
86
|
+
from pytcl.dynamic_estimation.particle_filters import bootstrap_pf_step
|
|
87
|
+
from pytcl.dynamic_estimation.particle_filters import bootstrap_pf_update
|
|
88
|
+
from pytcl.dynamic_estimation.particle_filters import effective_sample_size
|
|
89
|
+
from pytcl.dynamic_estimation.particle_filters import gaussian_likelihood
|
|
90
|
+
from pytcl.dynamic_estimation.particle_filters import initialize_particles
|
|
91
|
+
from pytcl.dynamic_estimation.particle_filters import particle_covariance
|
|
92
|
+
from pytcl.dynamic_estimation.particle_filters import particle_mean
|
|
93
|
+
from pytcl.dynamic_estimation.particle_filters import resample_multinomial
|
|
94
|
+
from pytcl.dynamic_estimation.particle_filters import resample_residual
|
|
95
|
+
from pytcl.dynamic_estimation.particle_filters import resample_systematic
|
|
103
96
|
|
|
104
97
|
# Smoothers
|
|
105
|
-
from pytcl.dynamic_estimation.smoothers import
|
|
106
|
-
|
|
107
|
-
|
|
108
|
-
|
|
109
|
-
|
|
110
|
-
|
|
111
|
-
|
|
112
|
-
|
|
113
|
-
two_filter_smoother,
|
|
114
|
-
)
|
|
98
|
+
from pytcl.dynamic_estimation.smoothers import FixedLagResult
|
|
99
|
+
from pytcl.dynamic_estimation.smoothers import RTSResult
|
|
100
|
+
from pytcl.dynamic_estimation.smoothers import SmoothedState
|
|
101
|
+
from pytcl.dynamic_estimation.smoothers import fixed_interval_smoother
|
|
102
|
+
from pytcl.dynamic_estimation.smoothers import fixed_lag_smoother
|
|
103
|
+
from pytcl.dynamic_estimation.smoothers import rts_smoother
|
|
104
|
+
from pytcl.dynamic_estimation.smoothers import rts_smoother_single_step
|
|
105
|
+
from pytcl.dynamic_estimation.smoothers import two_filter_smoother
|
|
115
106
|
|
|
116
107
|
# Re-export commonly used functions at the top level
|
|
117
108
|
|