nrl-tracker 0.22.2__py3-none-any.whl → 0.22.4__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.22.2.dist-info → nrl_tracker-0.22.4.dist-info}/METADATA +1 -1
- {nrl_tracker-0.22.2.dist-info → nrl_tracker-0.22.4.dist-info}/RECORD +69 -69
- pytcl/__init__.py +1 -1
- pytcl/assignment_algorithms/gating.py +3 -3
- pytcl/assignment_algorithms/jpda.py +12 -4
- pytcl/assignment_algorithms/two_dimensional/kbest.py +3 -1
- pytcl/astronomical/ephemerides.py +17 -9
- pytcl/astronomical/lambert.py +14 -4
- pytcl/astronomical/orbital_mechanics.py +3 -1
- pytcl/astronomical/reference_frames.py +3 -1
- pytcl/astronomical/relativity.py +8 -2
- pytcl/atmosphere/models.py +3 -1
- pytcl/clustering/gaussian_mixture.py +8 -4
- pytcl/clustering/hierarchical.py +5 -1
- pytcl/clustering/kmeans.py +3 -1
- pytcl/containers/cluster_set.py +9 -3
- pytcl/containers/measurement_set.py +6 -2
- pytcl/containers/rtree.py +5 -2
- pytcl/coordinate_systems/conversions/geodetic.py +15 -3
- pytcl/coordinate_systems/projections/projections.py +38 -11
- pytcl/coordinate_systems/rotations/rotations.py +3 -1
- pytcl/core/array_utils.py +4 -1
- pytcl/core/constants.py +3 -1
- pytcl/core/validation.py +17 -6
- pytcl/dynamic_estimation/imm.py +9 -3
- pytcl/dynamic_estimation/kalman/square_root.py +6 -2
- pytcl/dynamic_estimation/particle_filters/bootstrap.py +6 -2
- pytcl/dynamic_estimation/smoothers.py +3 -1
- pytcl/dynamic_models/process_noise/polynomial.py +6 -2
- pytcl/gravity/clenshaw.py +6 -2
- pytcl/gravity/egm.py +3 -1
- pytcl/gravity/models.py +3 -1
- pytcl/gravity/spherical_harmonics.py +10 -4
- pytcl/gravity/tides.py +16 -7
- pytcl/magnetism/emm.py +12 -3
- pytcl/magnetism/wmm.py +9 -2
- pytcl/mathematical_functions/basic_matrix/decompositions.py +3 -1
- pytcl/mathematical_functions/combinatorics/combinatorics.py +3 -1
- pytcl/mathematical_functions/geometry/geometry.py +12 -4
- pytcl/mathematical_functions/interpolation/interpolation.py +3 -1
- pytcl/mathematical_functions/signal_processing/detection.py +6 -3
- pytcl/mathematical_functions/signal_processing/filters.py +6 -2
- pytcl/mathematical_functions/signal_processing/matched_filter.py +4 -2
- pytcl/mathematical_functions/special_functions/elliptic.py +3 -1
- pytcl/mathematical_functions/special_functions/gamma_functions.py +3 -1
- pytcl/mathematical_functions/special_functions/lambert_w.py +3 -1
- pytcl/mathematical_functions/statistics/distributions.py +36 -12
- pytcl/mathematical_functions/statistics/estimators.py +3 -1
- pytcl/mathematical_functions/transforms/stft.py +9 -3
- pytcl/mathematical_functions/transforms/wavelets.py +18 -6
- pytcl/navigation/geodesy.py +31 -9
- pytcl/navigation/great_circle.py +12 -4
- pytcl/navigation/ins.py +8 -2
- pytcl/navigation/ins_gnss.py +25 -7
- pytcl/navigation/rhumb.py +10 -3
- pytcl/performance_evaluation/track_metrics.py +3 -1
- pytcl/plotting/coordinates.py +17 -5
- pytcl/plotting/metrics.py +9 -3
- pytcl/plotting/tracks.py +11 -3
- pytcl/static_estimation/least_squares.py +2 -1
- pytcl/static_estimation/maximum_likelihood.py +3 -3
- pytcl/terrain/dem.py +8 -2
- pytcl/terrain/loaders.py +13 -5
- pytcl/terrain/visibility.py +7 -2
- pytcl/trackers/hypothesis.py +7 -2
- pytcl/trackers/mht.py +15 -5
- {nrl_tracker-0.22.2.dist-info → nrl_tracker-0.22.4.dist-info}/LICENSE +0 -0
- {nrl_tracker-0.22.2.dist-info → nrl_tracker-0.22.4.dist-info}/WHEEL +0 -0
- {nrl_tracker-0.22.2.dist-info → nrl_tracker-0.22.4.dist-info}/top_level.txt +0 -0
pytcl/containers/rtree.py
CHANGED
|
@@ -281,7 +281,9 @@ class RTree:
|
|
|
281
281
|
|
|
282
282
|
# Simple split: sort by center x-coordinate and split in half
|
|
283
283
|
if node.is_leaf:
|
|
284
|
-
sorted_entries = sorted(
|
|
284
|
+
sorted_entries = sorted(
|
|
285
|
+
entries, key=lambda e: e[0].center[0] if e[0] else 0
|
|
286
|
+
)
|
|
285
287
|
else:
|
|
286
288
|
sorted_entries = sorted(
|
|
287
289
|
entries,
|
|
@@ -479,7 +481,8 @@ class RTree:
|
|
|
479
481
|
else:
|
|
480
482
|
# Sort children by distance and search closest first
|
|
481
483
|
child_dists = [
|
|
482
|
-
(min_dist_to_box(query, c.bbox) if c.bbox else np.inf, c)
|
|
484
|
+
(min_dist_to_box(query, c.bbox) if c.bbox else np.inf, c)
|
|
485
|
+
for c in node.children
|
|
483
486
|
]
|
|
484
487
|
child_dists.sort(key=lambda x: x[0])
|
|
485
488
|
for _, child in child_dists:
|
|
@@ -147,7 +147,9 @@ def ecef2geodetic(
|
|
|
147
147
|
# Bowring's iterative method
|
|
148
148
|
# Initial estimate
|
|
149
149
|
theta = np.arctan2(z * a, p * b)
|
|
150
|
-
lat = np.arctan2(
|
|
150
|
+
lat = np.arctan2(
|
|
151
|
+
z + ep2 * b * np.sin(theta) ** 3, p - e2 * a * np.cos(theta) ** 3
|
|
152
|
+
)
|
|
151
153
|
|
|
152
154
|
# Iterate for improved accuracy
|
|
153
155
|
for _ in range(5):
|
|
@@ -376,8 +378,18 @@ def enu2ecef(
|
|
|
376
378
|
cos_lon = np.cos(lon_ref)
|
|
377
379
|
|
|
378
380
|
# ECEF = R^T @ ENU + ECEF_ref
|
|
379
|
-
x =
|
|
380
|
-
|
|
381
|
+
x = (
|
|
382
|
+
-sin_lon * east
|
|
383
|
+
- sin_lat * cos_lon * north
|
|
384
|
+
+ cos_lat * cos_lon * up
|
|
385
|
+
+ ecef_ref[0]
|
|
386
|
+
)
|
|
387
|
+
y = (
|
|
388
|
+
cos_lon * east
|
|
389
|
+
- sin_lat * sin_lon * north
|
|
390
|
+
+ cos_lat * sin_lon * up
|
|
391
|
+
+ ecef_ref[1]
|
|
392
|
+
)
|
|
381
393
|
z = cos_lat * north + sin_lat * up + ecef_ref[2]
|
|
382
394
|
|
|
383
395
|
if x.size == 1:
|
|
@@ -140,7 +140,9 @@ def mercator(
|
|
|
140
140
|
|
|
141
141
|
# Northing using isometric latitude
|
|
142
142
|
sin_lat = np.sin(lat)
|
|
143
|
-
y = a * np.log(
|
|
143
|
+
y = a * np.log(
|
|
144
|
+
np.tan(np.pi / 4 + lat / 2) * ((1 - e * sin_lat) / (1 + e * sin_lat)) ** (e / 2)
|
|
145
|
+
)
|
|
144
146
|
|
|
145
147
|
# Scale factor
|
|
146
148
|
cos_lat = np.cos(lat)
|
|
@@ -201,7 +203,9 @@ def mercator_inverse(
|
|
|
201
203
|
|
|
202
204
|
for _ in range(max_iter):
|
|
203
205
|
sin_lat = np.sin(lat)
|
|
204
|
-
lat_new = np.pi / 2 - 2 * np.arctan(
|
|
206
|
+
lat_new = np.pi / 2 - 2 * np.arctan(
|
|
207
|
+
t * ((1 - e * sin_lat) / (1 + e * sin_lat)) ** (e / 2)
|
|
208
|
+
)
|
|
205
209
|
if abs(lat_new - lat) < tol:
|
|
206
210
|
break
|
|
207
211
|
lat = lat_new
|
|
@@ -579,7 +583,9 @@ def geodetic2utm(lat: float, lon: float, zone: Optional[int] = None) -> UTMResul
|
|
|
579
583
|
northing = result.y + 10000000.0
|
|
580
584
|
hemisphere = "S"
|
|
581
585
|
|
|
582
|
-
return UTMResult(
|
|
586
|
+
return UTMResult(
|
|
587
|
+
easting, northing, zone, hemisphere, result.scale, result.convergence
|
|
588
|
+
)
|
|
583
589
|
|
|
584
590
|
|
|
585
591
|
def utm2geodetic(
|
|
@@ -690,7 +696,8 @@ def stereographic(
|
|
|
690
696
|
return (
|
|
691
697
|
2
|
|
692
698
|
* np.arctan(
|
|
693
|
-
np.tan(np.pi / 4 + phi / 2)
|
|
699
|
+
np.tan(np.pi / 4 + phi / 2)
|
|
700
|
+
* ((1 - e * sin_phi) / (1 + e * sin_phi)) ** (e / 2)
|
|
694
701
|
)
|
|
695
702
|
- np.pi / 2
|
|
696
703
|
)
|
|
@@ -778,7 +785,8 @@ def stereographic_inverse(
|
|
|
778
785
|
chi0 = (
|
|
779
786
|
2
|
|
780
787
|
* np.arctan(
|
|
781
|
-
np.tan(np.pi / 4 + lat0 / 2)
|
|
788
|
+
np.tan(np.pi / 4 + lat0 / 2)
|
|
789
|
+
* ((1 - e * sin_lat0) / (1 + e * sin_lat0)) ** (e / 2)
|
|
782
790
|
)
|
|
783
791
|
- np.pi / 2
|
|
784
792
|
)
|
|
@@ -813,7 +821,8 @@ def stereographic_inverse(
|
|
|
813
821
|
lat_new = (
|
|
814
822
|
2
|
|
815
823
|
* np.arctan(
|
|
816
|
-
np.tan(np.pi / 4 + chi / 2)
|
|
824
|
+
np.tan(np.pi / 4 + chi / 2)
|
|
825
|
+
* ((1 + e * sin_lat) / (1 - e * sin_lat)) ** (e / 2)
|
|
817
826
|
)
|
|
818
827
|
- np.pi / 2
|
|
819
828
|
)
|
|
@@ -945,7 +954,9 @@ def lambert_conformal_conic(
|
|
|
945
954
|
|
|
946
955
|
def compute_t(phi: float) -> float:
|
|
947
956
|
sin_phi = np.sin(phi)
|
|
948
|
-
return np.tan(np.pi / 4 - phi / 2) / (
|
|
957
|
+
return np.tan(np.pi / 4 - phi / 2) / (
|
|
958
|
+
((1 - e * sin_phi) / (1 + e * sin_phi)) ** (e / 2)
|
|
959
|
+
)
|
|
949
960
|
|
|
950
961
|
m1 = compute_m(lat1)
|
|
951
962
|
m2 = compute_m(lat2)
|
|
@@ -1034,7 +1045,9 @@ def lambert_conformal_conic_inverse(
|
|
|
1034
1045
|
|
|
1035
1046
|
def compute_t(phi: float) -> float:
|
|
1036
1047
|
sin_phi = np.sin(phi)
|
|
1037
|
-
return np.tan(np.pi / 4 - phi / 2) / (
|
|
1048
|
+
return np.tan(np.pi / 4 - phi / 2) / (
|
|
1049
|
+
((1 - e * sin_phi) / (1 + e * sin_phi)) ** (e / 2)
|
|
1050
|
+
)
|
|
1038
1051
|
|
|
1039
1052
|
m1 = compute_m(lat1)
|
|
1040
1053
|
m2 = compute_m(lat2)
|
|
@@ -1062,7 +1075,9 @@ def lambert_conformal_conic_inverse(
|
|
|
1062
1075
|
lat = np.pi / 2 - 2 * np.arctan(t)
|
|
1063
1076
|
for _ in range(max_iter):
|
|
1064
1077
|
sin_lat = np.sin(lat)
|
|
1065
|
-
lat_new = np.pi / 2 - 2 * np.arctan(
|
|
1078
|
+
lat_new = np.pi / 2 - 2 * np.arctan(
|
|
1079
|
+
t * ((1 - e * sin_lat) / (1 + e * sin_lat)) ** (e / 2)
|
|
1080
|
+
)
|
|
1066
1081
|
if abs(lat_new - lat) < tol:
|
|
1067
1082
|
break
|
|
1068
1083
|
lat = lat_new
|
|
@@ -1126,7 +1141,13 @@ def azimuthal_equidistant(
|
|
|
1126
1141
|
"""
|
|
1127
1142
|
# Use spherical approximation with authalic radius
|
|
1128
1143
|
R = a * np.sqrt(
|
|
1129
|
-
(
|
|
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
|
|
1130
1151
|
)
|
|
1131
1152
|
|
|
1132
1153
|
sin_lat = np.sin(lat)
|
|
@@ -1196,7 +1217,13 @@ def azimuthal_equidistant_inverse(
|
|
|
1196
1217
|
(latitude, longitude) in radians.
|
|
1197
1218
|
"""
|
|
1198
1219
|
R = a * np.sqrt(
|
|
1199
|
-
(
|
|
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
|
|
1200
1227
|
)
|
|
1201
1228
|
|
|
1202
1229
|
rho = np.sqrt(x**2 + y**2)
|
|
@@ -353,7 +353,9 @@ def rotmat2axisangle(
|
|
|
353
353
|
return axis / np.linalg.norm(axis), float(angle)
|
|
354
354
|
|
|
355
355
|
# General case
|
|
356
|
-
axis = np.array([R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]]) / (
|
|
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
359
|
|
|
358
360
|
return axis, float(angle)
|
|
359
361
|
|
pytcl/core/array_utils.py
CHANGED
|
@@ -374,7 +374,10 @@ def normalize_vector(
|
|
|
374
374
|
v: ArrayLike,
|
|
375
375
|
axis: int | None = None,
|
|
376
376
|
return_norm: bool = False,
|
|
377
|
-
) ->
|
|
377
|
+
) -> (
|
|
378
|
+
NDArray[np.floating[Any]]
|
|
379
|
+
| tuple[NDArray[np.floating[Any]], NDArray[np.floating[Any]]]
|
|
380
|
+
):
|
|
378
381
|
"""
|
|
379
382
|
Normalize vector(s) to unit length.
|
|
380
383
|
|
pytcl/core/constants.py
CHANGED
|
@@ -71,7 +71,9 @@ 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 / (
|
|
74
|
+
EARTH_ECCENTRICITY_PRIME_SQ: Final[float] = EARTH_ECCENTRICITY_SQ / (
|
|
75
|
+
1 - EARTH_ECCENTRICITY_SQ
|
|
76
|
+
)
|
|
75
77
|
|
|
76
78
|
#: Earth rotation rate [rad/s] (IERS Conventions 2010)
|
|
77
79
|
EARTH_ROTATION_RATE: Final[float] = 7.292115e-5
|
pytcl/core/validation.py
CHANGED
|
@@ -108,7 +108,9 @@ def validate_array(
|
|
|
108
108
|
if ndim is not None:
|
|
109
109
|
valid_ndims = (ndim,) if isinstance(ndim, int) else ndim
|
|
110
110
|
if result.ndim not in valid_ndims:
|
|
111
|
-
raise ValidationError(
|
|
111
|
+
raise ValidationError(
|
|
112
|
+
f"{name} must have {ndim} dimension(s), got {result.ndim}"
|
|
113
|
+
)
|
|
112
114
|
|
|
113
115
|
if min_ndim is not None and result.ndim < min_ndim:
|
|
114
116
|
raise ValidationError(
|
|
@@ -123,10 +125,14 @@ def validate_array(
|
|
|
123
125
|
# Check shape
|
|
124
126
|
if shape is not None:
|
|
125
127
|
if len(shape) != result.ndim:
|
|
126
|
-
raise ValidationError(
|
|
128
|
+
raise ValidationError(
|
|
129
|
+
f"{name} must have {len(shape)} dimensions, got {result.ndim}"
|
|
130
|
+
)
|
|
127
131
|
for i, (expected, actual) in enumerate(zip(shape, result.shape)):
|
|
128
132
|
if expected is not None and expected != actual:
|
|
129
|
-
raise ValidationError(
|
|
133
|
+
raise ValidationError(
|
|
134
|
+
f"{name} dimension {i} must be {expected}, got {actual}"
|
|
135
|
+
)
|
|
130
136
|
|
|
131
137
|
# Check finite
|
|
132
138
|
if finite and not np.all(np.isfinite(result)):
|
|
@@ -253,7 +259,9 @@ def ensure_row_vector(arr: ArrayLike, name: str = "vector") -> NDArray[Any]:
|
|
|
253
259
|
return result.reshape(1, -1)
|
|
254
260
|
elif result.ndim == 2:
|
|
255
261
|
if result.shape[0] != 1:
|
|
256
|
-
raise ValidationError(
|
|
262
|
+
raise ValidationError(
|
|
263
|
+
f"{name} must be a row vector (1, n), got shape {result.shape}"
|
|
264
|
+
)
|
|
257
265
|
return result
|
|
258
266
|
else:
|
|
259
267
|
raise ValidationError(f"{name} must be 1D or 2D, got {result.ndim}D")
|
|
@@ -366,7 +374,8 @@ def ensure_positive_definite(
|
|
|
366
374
|
|
|
367
375
|
if min_eigenvalue < threshold:
|
|
368
376
|
raise ValidationError(
|
|
369
|
-
f"{name} must be positive definite, "
|
|
377
|
+
f"{name} must be positive definite, "
|
|
378
|
+
f"minimum eigenvalue is {min_eigenvalue:.2e}"
|
|
370
379
|
)
|
|
371
380
|
|
|
372
381
|
return result
|
|
@@ -398,7 +407,9 @@ def validate_same_shape(*arrays: ArrayLike, names: Sequence[str] | None = None)
|
|
|
398
407
|
|
|
399
408
|
if not all(s == shapes[0] for s in shapes):
|
|
400
409
|
shape_strs = [f"{name}: {shape}" for name, shape in zip(names, shapes)]
|
|
401
|
-
raise ValidationError(
|
|
410
|
+
raise ValidationError(
|
|
411
|
+
f"Arrays must have the same shape. Got: {', '.join(shape_strs)}"
|
|
412
|
+
)
|
|
402
413
|
|
|
403
414
|
|
|
404
415
|
def validated_array_input(
|
pytcl/dynamic_estimation/imm.py
CHANGED
|
@@ -469,8 +469,12 @@ def imm_predict_update(
|
|
|
469
469
|
result : IMMUpdate
|
|
470
470
|
Updated states, covariances, and mode probabilities.
|
|
471
471
|
"""
|
|
472
|
-
pred = imm_predict(
|
|
473
|
-
|
|
472
|
+
pred = imm_predict(
|
|
473
|
+
mode_states, mode_covs, mode_probs, transition_matrix, F_list, Q_list
|
|
474
|
+
)
|
|
475
|
+
return imm_update(
|
|
476
|
+
pred.mode_states, pred.mode_covs, pred.mode_probs, z, H_list, R_list
|
|
477
|
+
)
|
|
474
478
|
|
|
475
479
|
|
|
476
480
|
class IMMEstimator:
|
|
@@ -672,7 +676,9 @@ class IMMEstimator:
|
|
|
672
676
|
Update result.
|
|
673
677
|
"""
|
|
674
678
|
if not self.H_list:
|
|
675
|
-
raise ValueError(
|
|
679
|
+
raise ValueError(
|
|
680
|
+
"Measurement model not set. Call set_measurement_model first."
|
|
681
|
+
)
|
|
676
682
|
|
|
677
683
|
result = imm_update(
|
|
678
684
|
self.mode_states,
|
|
@@ -705,7 +705,9 @@ def ud_update(
|
|
|
705
705
|
D_upd = D.copy()
|
|
706
706
|
|
|
707
707
|
for i in range(m):
|
|
708
|
-
x_upd, U_upd, D_upd = ud_update_scalar(
|
|
708
|
+
x_upd, U_upd, D_upd = ud_update_scalar(
|
|
709
|
+
x_upd, U_upd, D_upd, z[i], H[i, :], R[i, i]
|
|
710
|
+
)
|
|
709
711
|
else:
|
|
710
712
|
# Decorrelate measurements
|
|
711
713
|
S_R = np.linalg.cholesky(R)
|
|
@@ -718,7 +720,9 @@ def ud_update(
|
|
|
718
720
|
D_upd = D.copy()
|
|
719
721
|
|
|
720
722
|
for i in range(m):
|
|
721
|
-
x_upd, U_upd, D_upd = ud_update_scalar(
|
|
723
|
+
x_upd, U_upd, D_upd = ud_update_scalar(
|
|
724
|
+
x_upd, U_upd, D_upd, z_dec[i], H_dec[i, :], 1.0
|
|
725
|
+
)
|
|
722
726
|
|
|
723
727
|
# Compute likelihood
|
|
724
728
|
P = ud_reconstruct(U, D)
|
|
@@ -157,7 +157,9 @@ def resample_residual(
|
|
|
157
157
|
residual = Nw - floor_Nw
|
|
158
158
|
|
|
159
159
|
# Deterministic copies (JIT-compiled)
|
|
160
|
-
resampled, idx = _resample_residual_deterministic(
|
|
160
|
+
resampled, idx = _resample_residual_deterministic(
|
|
161
|
+
particles.astype(np.float64), floor_Nw
|
|
162
|
+
)
|
|
161
163
|
|
|
162
164
|
# Multinomial resampling of residuals
|
|
163
165
|
if idx < N:
|
|
@@ -269,7 +271,9 @@ def bootstrap_pf_update(
|
|
|
269
271
|
N = len(particles)
|
|
270
272
|
|
|
271
273
|
# Compute likelihoods
|
|
272
|
-
likelihoods = np.array(
|
|
274
|
+
likelihoods = np.array(
|
|
275
|
+
[likelihood_func(z, particles[i]) for i in range(N)], dtype=np.float64
|
|
276
|
+
)
|
|
273
277
|
|
|
274
278
|
# Update weights
|
|
275
279
|
weights_unnorm = weights * likelihoods
|
|
@@ -646,7 +646,9 @@ def rts_smoother_single_step(
|
|
|
646
646
|
result : SmoothedState
|
|
647
647
|
Smoothed state and covariance at current time.
|
|
648
648
|
"""
|
|
649
|
-
x_s, P_s = kf_smooth(
|
|
649
|
+
x_s, P_s = kf_smooth(
|
|
650
|
+
x_filt, P_filt, x_pred_next, P_pred_next, x_smooth_next, P_smooth_next, F
|
|
651
|
+
)
|
|
650
652
|
return SmoothedState(x=x_s, P=P_s)
|
|
651
653
|
|
|
652
654
|
|
|
@@ -71,7 +71,9 @@ def q_poly_kal(
|
|
|
71
71
|
|
|
72
72
|
# Q[i,j] = q * T^(pi+pj+1) / ((pi+pj+1) * pi! * pj!)
|
|
73
73
|
power = pi + pj + 1
|
|
74
|
-
Q_1d[i, j] =
|
|
74
|
+
Q_1d[i, j] = (
|
|
75
|
+
q * T**power / (power * math.factorial(pi) * math.factorial(pj))
|
|
76
|
+
)
|
|
75
77
|
|
|
76
78
|
if num_dims == 1:
|
|
77
79
|
return Q_1d
|
|
@@ -274,7 +276,9 @@ def q_continuous_white_noise(
|
|
|
274
276
|
"""
|
|
275
277
|
# This is the same as q_discrete_white_noise but with spectral density
|
|
276
278
|
# instead of variance (multiply by T for conversion in simple cases)
|
|
277
|
-
return q_discrete_white_noise(
|
|
279
|
+
return q_discrete_white_noise(
|
|
280
|
+
dim=dim, T=T, var=spectral_density, block_size=block_size
|
|
281
|
+
)
|
|
278
282
|
|
|
279
283
|
|
|
280
284
|
__all__ = [
|
pytcl/gravity/clenshaw.py
CHANGED
|
@@ -407,7 +407,9 @@ def clenshaw_potential(
|
|
|
407
407
|
V = 0.0
|
|
408
408
|
|
|
409
409
|
for m in range(n_max + 1):
|
|
410
|
-
sum_C, sum_S = clenshaw_sum_order(
|
|
410
|
+
sum_C, sum_S = clenshaw_sum_order(
|
|
411
|
+
m, cos_theta, sin_theta, C_scaled, S_scaled, n_max
|
|
412
|
+
)
|
|
411
413
|
|
|
412
414
|
cos_m_lon = np.cos(m * lon)
|
|
413
415
|
sin_m_lon = np.sin(m * lon)
|
|
@@ -496,7 +498,9 @@ def clenshaw_gravity(
|
|
|
496
498
|
|
|
497
499
|
for m in range(n_max + 1):
|
|
498
500
|
# Value sum
|
|
499
|
-
sum_C, sum_S = clenshaw_sum_order(
|
|
501
|
+
sum_C, sum_S = clenshaw_sum_order(
|
|
502
|
+
m, cos_theta, sin_theta, C_scaled, S_scaled, n_max
|
|
503
|
+
)
|
|
500
504
|
|
|
501
505
|
# Radial derivative sum
|
|
502
506
|
sum_C_r, sum_S_r = clenshaw_sum_order(
|
pytcl/gravity/egm.py
CHANGED
|
@@ -491,7 +491,9 @@ def geoid_heights(
|
|
|
491
491
|
# Compute for each point
|
|
492
492
|
heights = np.zeros(len(lats))
|
|
493
493
|
for i in range(len(lats)):
|
|
494
|
-
heights[i] = geoid_height(
|
|
494
|
+
heights[i] = geoid_height(
|
|
495
|
+
lats[i], lons[i], model, n_max, coefficients=coefficients
|
|
496
|
+
)
|
|
495
497
|
|
|
496
498
|
return heights
|
|
497
499
|
|
pytcl/gravity/models.py
CHANGED
|
@@ -190,7 +190,9 @@ def normal_gravity(
|
|
|
190
190
|
sin2_lat = np.sin(lat) ** 2
|
|
191
191
|
|
|
192
192
|
# Height correction
|
|
193
|
-
gamma = gamma_0 * (
|
|
193
|
+
gamma = gamma_0 * (
|
|
194
|
+
1 - 2 / a * (1 + f + m - 2 * f * sin2_lat) * h + 3 / (a * a) * h * h
|
|
195
|
+
)
|
|
194
196
|
|
|
195
197
|
return gamma
|
|
196
198
|
|
|
@@ -94,7 +94,9 @@ def associated_legendre(
|
|
|
94
94
|
b_nm = np.sqrt(((n - 1) ** 2 - m * m) / (4 * (n - 1) ** 2 - 1))
|
|
95
95
|
P[n, m] = a_nm * (x * P[n - 1, m] - b_nm * P[n - 2, m])
|
|
96
96
|
else:
|
|
97
|
-
P[n, m] = (
|
|
97
|
+
P[n, m] = (
|
|
98
|
+
(2 * n - 1) * x * P[n - 1, m] - (n + m - 1) * P[n - 2, m]
|
|
99
|
+
) / (n - m)
|
|
98
100
|
|
|
99
101
|
return P
|
|
100
102
|
|
|
@@ -155,7 +157,8 @@ def associated_legendre_derivative(
|
|
|
155
157
|
factor = np.sqrt((n - m) * (n + m + 1))
|
|
156
158
|
if m + 1 <= m_max and n >= m + 1:
|
|
157
159
|
dP[n, m] = (
|
|
158
|
-
n * x / u2 * P[n, m]
|
|
160
|
+
n * x / u2 * P[n, m]
|
|
161
|
+
- factor / np.sqrt(u2) * P[n, m + 1]
|
|
159
162
|
if m + 1 <= n
|
|
160
163
|
else n * x / u2 * P[n, m]
|
|
161
164
|
)
|
|
@@ -163,7 +166,9 @@ def associated_legendre_derivative(
|
|
|
163
166
|
dP[n, m] = n * x / u2 * P[n, m]
|
|
164
167
|
else:
|
|
165
168
|
# Unnormalized form
|
|
166
|
-
dP[n, m] = (
|
|
169
|
+
dP[n, m] = (
|
|
170
|
+
(n * x * P[n, m] - (n + m) * P[n - 1, m]) / u2 if n > 0 else 0
|
|
171
|
+
)
|
|
167
172
|
|
|
168
173
|
return dP
|
|
169
174
|
|
|
@@ -483,7 +488,8 @@ def associated_legendre_scaled(
|
|
|
483
488
|
s_ratio_2 = scale[n] / scale[n - 2]
|
|
484
489
|
|
|
485
490
|
P_scaled[n, m] = a_nm * (
|
|
486
|
-
x * P_scaled[n - 1, m] * s_ratio_1
|
|
491
|
+
x * P_scaled[n - 1, m] * s_ratio_1
|
|
492
|
+
- b_nm * P_scaled[n - 2, m] * s_ratio_2
|
|
487
493
|
)
|
|
488
494
|
|
|
489
495
|
return P_scaled, scale_exp
|
pytcl/gravity/tides.py
CHANGED
|
@@ -179,31 +179,36 @@ def fundamental_arguments(T: float) -> Tuple[float, float, float, float, float]:
|
|
|
179
179
|
# Mean anomaly of the Moon (l)
|
|
180
180
|
l_moon = (
|
|
181
181
|
134.96340251
|
|
182
|
-
+ (1717915923.2178 * T + 31.8792 * T**2 + 0.051635 * T**3 - 0.00024470 * T**4)
|
|
182
|
+
+ (1717915923.2178 * T + 31.8792 * T**2 + 0.051635 * T**3 - 0.00024470 * T**4)
|
|
183
|
+
/ 3600.0
|
|
183
184
|
) * deg2rad
|
|
184
185
|
|
|
185
186
|
# Mean anomaly of the Sun (l')
|
|
186
187
|
l_sun = (
|
|
187
188
|
357.52910918
|
|
188
|
-
+ (129596581.0481 * T - 0.5532 * T**2 + 0.000136 * T**3 - 0.00001149 * T**4)
|
|
189
|
+
+ (129596581.0481 * T - 0.5532 * T**2 + 0.000136 * T**3 - 0.00001149 * T**4)
|
|
190
|
+
/ 3600.0
|
|
189
191
|
) * deg2rad
|
|
190
192
|
|
|
191
193
|
# Mean argument of latitude of the Moon (F)
|
|
192
194
|
F = (
|
|
193
195
|
93.27209062
|
|
194
|
-
+ (1739527262.8478 * T - 12.7512 * T**2 - 0.001037 * T**3 + 0.00000417 * T**4)
|
|
196
|
+
+ (1739527262.8478 * T - 12.7512 * T**2 - 0.001037 * T**3 + 0.00000417 * T**4)
|
|
197
|
+
/ 3600.0
|
|
195
198
|
) * deg2rad
|
|
196
199
|
|
|
197
200
|
# Mean elongation of the Moon from the Sun (D)
|
|
198
201
|
D = (
|
|
199
202
|
297.85019547
|
|
200
|
-
+ (1602961601.2090 * T - 6.3706 * T**2 + 0.006593 * T**3 - 0.00003169 * T**4)
|
|
203
|
+
+ (1602961601.2090 * T - 6.3706 * T**2 + 0.006593 * T**3 - 0.00003169 * T**4)
|
|
204
|
+
/ 3600.0
|
|
201
205
|
) * deg2rad
|
|
202
206
|
|
|
203
207
|
# Mean longitude of the ascending node of the Moon (Omega)
|
|
204
208
|
Omega = (
|
|
205
209
|
125.04455501
|
|
206
|
-
+ (-6962890.5431 * T + 7.4722 * T**2 + 0.007702 * T**3 - 0.00005939 * T**4)
|
|
210
|
+
+ (-6962890.5431 * T + 7.4722 * T**2 + 0.007702 * T**3 - 0.00005939 * T**4)
|
|
211
|
+
/ 3600.0
|
|
207
212
|
) * deg2rad
|
|
208
213
|
|
|
209
214
|
return (
|
|
@@ -274,7 +279,9 @@ def moon_position_approximate(mjd: float) -> Tuple[float, float, float]:
|
|
|
274
279
|
|
|
275
280
|
# Distance perturbations (km)
|
|
276
281
|
r_pert = (
|
|
277
|
-
-20.905355 * np.cos(M_prime)
|
|
282
|
+
-20.905355 * np.cos(M_prime)
|
|
283
|
+
- 3.699111 * np.cos(2 * D - M_prime)
|
|
284
|
+
- 2.955968 * np.cos(2 * D)
|
|
278
285
|
) * 1000 # Convert to km
|
|
279
286
|
|
|
280
287
|
# Final position
|
|
@@ -579,7 +586,9 @@ def solid_earth_tide_gravity(
|
|
|
579
586
|
delta_g_north = 0.0
|
|
580
587
|
delta_g_east = 0.0
|
|
581
588
|
|
|
582
|
-
return TidalGravity(
|
|
589
|
+
return TidalGravity(
|
|
590
|
+
delta_g=delta_g, delta_g_north=delta_g_north, delta_g_east=delta_g_east
|
|
591
|
+
)
|
|
583
592
|
|
|
584
593
|
|
|
585
594
|
def ocean_tide_loading_displacement(
|
pytcl/magnetism/emm.py
CHANGED
|
@@ -531,7 +531,8 @@ def _high_res_field_spherical(
|
|
|
531
531
|
factor = np.sqrt((n - m) * (n + m + 1))
|
|
532
532
|
if m + 1 <= n:
|
|
533
533
|
dP[n, m] = (
|
|
534
|
-
n * cos_theta / sin_theta * P[n, m]
|
|
534
|
+
n * cos_theta / sin_theta * P[n, m]
|
|
535
|
+
- factor * P[n, m + 1] / sin_theta
|
|
535
536
|
if m + 1 <= n_max_eval
|
|
536
537
|
else n * cos_theta / sin_theta * P[n, m]
|
|
537
538
|
)
|
|
@@ -558,7 +559,13 @@ def _high_res_field_spherical(
|
|
|
558
559
|
B_theta += -r_power * dP[n, m] * (gnm * cos_m_lon + hnm * sin_m_lon)
|
|
559
560
|
|
|
560
561
|
if abs(sin_theta) > 1e-10:
|
|
561
|
-
B_phi +=
|
|
562
|
+
B_phi += (
|
|
563
|
+
r_power
|
|
564
|
+
* m
|
|
565
|
+
* P[n, m]
|
|
566
|
+
/ sin_theta
|
|
567
|
+
* (gnm * sin_m_lon - hnm * cos_m_lon)
|
|
568
|
+
)
|
|
562
569
|
|
|
563
570
|
return B_r, B_theta, B_phi
|
|
564
571
|
|
|
@@ -617,7 +624,9 @@ def emm(
|
|
|
617
624
|
r = a + h
|
|
618
625
|
|
|
619
626
|
# Compute field in spherical coordinates
|
|
620
|
-
B_r, B_theta, B_phi = _high_res_field_spherical(
|
|
627
|
+
B_r, B_theta, B_phi = _high_res_field_spherical(
|
|
628
|
+
lat_gc, lon, r, year, coefficients, n_max
|
|
629
|
+
)
|
|
621
630
|
|
|
622
631
|
# Convert to geodetic coordinates
|
|
623
632
|
X = -B_theta # North
|
pytcl/magnetism/wmm.py
CHANGED
|
@@ -468,7 +468,8 @@ def magnetic_field_spherical(
|
|
|
468
468
|
factor = np.sqrt((n - m) * (n + m + 1))
|
|
469
469
|
if m + 1 <= n:
|
|
470
470
|
dP[n, m] = (
|
|
471
|
-
n * cos_theta / sin_theta * P[n, m]
|
|
471
|
+
n * cos_theta / sin_theta * P[n, m]
|
|
472
|
+
- factor * P[n, m + 1] / sin_theta
|
|
472
473
|
if m + 1 <= n_max
|
|
473
474
|
else n * cos_theta / sin_theta * P[n, m]
|
|
474
475
|
)
|
|
@@ -498,7 +499,13 @@ def magnetic_field_spherical(
|
|
|
498
499
|
B_theta += -r_power * dP[n, m] * (gnm * cos_m_lon + hnm * sin_m_lon)
|
|
499
500
|
|
|
500
501
|
if abs(sin_theta) > 1e-10:
|
|
501
|
-
B_phi +=
|
|
502
|
+
B_phi += (
|
|
503
|
+
r_power
|
|
504
|
+
* m
|
|
505
|
+
* P[n, m]
|
|
506
|
+
/ sin_theta
|
|
507
|
+
* (gnm * sin_m_lon - hnm * cos_m_lon)
|
|
508
|
+
)
|
|
502
509
|
|
|
503
510
|
return B_r, B_theta, B_phi
|
|
504
511
|
|
|
@@ -160,7 +160,9 @@ def tria_sqrt(
|
|
|
160
160
|
if B is not None:
|
|
161
161
|
B = np.asarray(B, dtype=np.float64)
|
|
162
162
|
if A.shape[0] != B.shape[0]:
|
|
163
|
-
raise ValueError(
|
|
163
|
+
raise ValueError(
|
|
164
|
+
f"A and B must have same number of rows: {A.shape[0]} vs {B.shape[0]}"
|
|
165
|
+
)
|
|
164
166
|
combined = np.hstack([A, B])
|
|
165
167
|
else:
|
|
166
168
|
combined = A
|
|
@@ -378,7 +378,9 @@ def partitions(n: int, k: Optional[int] = None) -> Iterator[Tuple[int, ...]]:
|
|
|
378
378
|
[(4,), (3, 1), (2, 2), (2, 1, 1), (1, 1, 1, 1)]
|
|
379
379
|
"""
|
|
380
380
|
|
|
381
|
-
def gen_partitions(
|
|
381
|
+
def gen_partitions(
|
|
382
|
+
n: int, max_val: int, prefix: Tuple[int, ...]
|
|
383
|
+
) -> Iterator[Tuple[int, ...]]:
|
|
382
384
|
if n == 0:
|
|
383
385
|
yield prefix
|
|
384
386
|
return
|
|
@@ -191,8 +191,12 @@ def polygon_centroid(vertices: ArrayLike) -> NDArray[np.floating]:
|
|
|
191
191
|
|
|
192
192
|
# Centroid
|
|
193
193
|
factor = 1.0 / (3.0 * a)
|
|
194
|
-
cx = factor * np.sum(
|
|
195
|
-
|
|
194
|
+
cx = factor * np.sum(
|
|
195
|
+
(x + np.roll(x, -1)) * (x * np.roll(y, -1) - np.roll(x, -1) * y)
|
|
196
|
+
)
|
|
197
|
+
cy = factor * np.sum(
|
|
198
|
+
(y + np.roll(y, -1)) * (x * np.roll(y, -1) - np.roll(x, -1) * y)
|
|
199
|
+
)
|
|
196
200
|
|
|
197
201
|
return np.array([cx, cy], dtype=np.float64)
|
|
198
202
|
|
|
@@ -547,10 +551,14 @@ def minimum_bounding_circle(
|
|
|
547
551
|
return circle_from_two_points(p1, p3)
|
|
548
552
|
|
|
549
553
|
ux = (
|
|
550
|
-
(ax**2 + ay**2) * (by - cy)
|
|
554
|
+
(ax**2 + ay**2) * (by - cy)
|
|
555
|
+
+ (bx**2 + by**2) * (cy - ay)
|
|
556
|
+
+ (cx**2 + cy**2) * (ay - by)
|
|
551
557
|
) / d
|
|
552
558
|
uy = (
|
|
553
|
-
(ax**2 + ay**2) * (cx - bx)
|
|
559
|
+
(ax**2 + ay**2) * (cx - bx)
|
|
560
|
+
+ (bx**2 + by**2) * (ax - cx)
|
|
561
|
+
+ (cx**2 + cy**2) * (bx - ax)
|
|
554
562
|
) / d
|
|
555
563
|
|
|
556
564
|
center = np.array([ux, uy])
|
|
@@ -371,7 +371,9 @@ def rbf_interpolate(
|
|
|
371
371
|
points = np.asarray(points, dtype=np.float64)
|
|
372
372
|
values = np.asarray(values, dtype=np.float64)
|
|
373
373
|
|
|
374
|
-
return interpolate.RBFInterpolator(
|
|
374
|
+
return interpolate.RBFInterpolator(
|
|
375
|
+
points, values, kernel=kernel, smoothing=smoothing
|
|
376
|
+
)
|
|
375
377
|
|
|
376
378
|
|
|
377
379
|
def barycentric(
|