nrl-tracker 0.22.3__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.3.dist-info → nrl_tracker-0.22.4.dist-info}/METADATA +1 -1
- {nrl_tracker-0.22.3.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.3.dist-info → nrl_tracker-0.22.4.dist-info}/LICENSE +0 -0
- {nrl_tracker-0.22.3.dist-info → nrl_tracker-0.22.4.dist-info}/WHEEL +0 -0
- {nrl_tracker-0.22.3.dist-info → nrl_tracker-0.22.4.dist-info}/top_level.txt +0 -0
|
@@ -410,7 +410,8 @@ def _cfar_2d_ca_kernel(
|
|
|
410
410
|
for ri in range(row_min, row_max):
|
|
411
411
|
for ci in range(col_min, col_max):
|
|
412
412
|
if not (
|
|
413
|
-
guard_row_min <= ri < guard_row_max
|
|
413
|
+
guard_row_min <= ri < guard_row_max
|
|
414
|
+
and guard_col_min <= ci < guard_col_max
|
|
414
415
|
):
|
|
415
416
|
ref_sum += image[ri, ci]
|
|
416
417
|
n_cells += 1
|
|
@@ -459,7 +460,8 @@ def _cfar_2d_go_kernel(
|
|
|
459
460
|
for ri in range(row_min, row_max):
|
|
460
461
|
for ci in range(col_min, col_max):
|
|
461
462
|
if not (
|
|
462
|
-
guard_row_min <= ri < guard_row_max
|
|
463
|
+
guard_row_min <= ri < guard_row_max
|
|
464
|
+
and guard_col_min <= ci < guard_col_max
|
|
463
465
|
):
|
|
464
466
|
if ri < i:
|
|
465
467
|
top_sum += image[ri, ci]
|
|
@@ -510,7 +512,8 @@ def _cfar_2d_so_kernel(
|
|
|
510
512
|
for ri in range(row_min, row_max):
|
|
511
513
|
for ci in range(col_min, col_max):
|
|
512
514
|
if not (
|
|
513
|
-
guard_row_min <= ri < guard_row_max
|
|
515
|
+
guard_row_min <= ri < guard_row_max
|
|
516
|
+
and guard_col_min <= ci < guard_col_max
|
|
514
517
|
):
|
|
515
518
|
if ri < i:
|
|
516
519
|
top_sum += image[ri, ci]
|
|
@@ -606,9 +606,13 @@ def filtfilt(
|
|
|
606
606
|
|
|
607
607
|
if isinstance(coeffs, FilterCoefficients):
|
|
608
608
|
if coeffs.sos is not None:
|
|
609
|
-
return scipy_signal.sosfiltfilt(
|
|
609
|
+
return scipy_signal.sosfiltfilt(
|
|
610
|
+
coeffs.sos, x, padtype=padtype, padlen=padlen
|
|
611
|
+
)
|
|
610
612
|
else:
|
|
611
|
-
return scipy_signal.filtfilt(
|
|
613
|
+
return scipy_signal.filtfilt(
|
|
614
|
+
coeffs.b, coeffs.a, x, padtype=padtype, padlen=padlen
|
|
615
|
+
)
|
|
612
616
|
elif isinstance(coeffs, tuple) and len(coeffs) == 2:
|
|
613
617
|
b, a = coeffs
|
|
614
618
|
return scipy_signal.filtfilt(b, a, x, padtype=padtype, padlen=padlen)
|
|
@@ -590,7 +590,8 @@ def _ambiguity_function_kernel(
|
|
|
590
590
|
for k in range(n_signal - delay_samples):
|
|
591
591
|
s1 = signal[k]
|
|
592
592
|
s2_conj = (
|
|
593
|
-
shifted[delay_samples + k].real
|
|
593
|
+
shifted[delay_samples + k].real
|
|
594
|
+
- 1j * shifted[delay_samples + k].imag
|
|
594
595
|
)
|
|
595
596
|
result += s1 * s2_conj
|
|
596
597
|
|
|
@@ -637,7 +638,8 @@ def _cross_ambiguity_kernel(
|
|
|
637
638
|
for k in range(n_signal - delay_samples):
|
|
638
639
|
s1 = signal1[k]
|
|
639
640
|
s2_conj = (
|
|
640
|
-
shifted[delay_samples + k].real
|
|
641
|
+
shifted[delay_samples + k].real
|
|
642
|
+
- 1j * shifted[delay_samples + k].imag
|
|
641
643
|
)
|
|
642
644
|
result += s1 * s2_conj
|
|
643
645
|
|
|
@@ -243,7 +243,9 @@ def elliprg(x: ArrayLike, y: ArrayLike, z: ArrayLike) -> NDArray[np.floating]:
|
|
|
243
243
|
return np.asarray(sp.elliprg(x, y, z), dtype=np.float64)
|
|
244
244
|
|
|
245
245
|
|
|
246
|
-
def elliprj(
|
|
246
|
+
def elliprj(
|
|
247
|
+
x: ArrayLike, y: ArrayLike, z: ArrayLike, p: ArrayLike
|
|
248
|
+
) -> NDArray[np.floating]:
|
|
247
249
|
"""
|
|
248
250
|
Carlson symmetric elliptic integral R_J.
|
|
249
251
|
|
|
@@ -421,7 +421,9 @@ def comb(
|
|
|
421
421
|
--------
|
|
422
422
|
scipy.special.comb : Combinations.
|
|
423
423
|
"""
|
|
424
|
-
return np.asarray(
|
|
424
|
+
return np.asarray(
|
|
425
|
+
sp.comb(n, k, exact=exact, repetition=repetition), dtype=np.float64
|
|
426
|
+
)
|
|
425
427
|
|
|
426
428
|
|
|
427
429
|
def perm(n: ArrayLike, k: ArrayLike, exact: bool = False) -> NDArray:
|
|
@@ -108,7 +108,9 @@ def lambert_w_real(
|
|
|
108
108
|
raise ValueError(f"For branch 0, x must be >= -1/e ≈ {branch_point:.6f}")
|
|
109
109
|
elif branch == -1:
|
|
110
110
|
if np.any((x < branch_point) | (x >= 0)):
|
|
111
|
-
raise ValueError(
|
|
111
|
+
raise ValueError(
|
|
112
|
+
f"For branch -1, x must be in [-1/e, 0) ≈ [{branch_point:.6f}, 0)"
|
|
113
|
+
)
|
|
112
114
|
else:
|
|
113
115
|
raise ValueError(f"branch must be 0 or -1, got {branch}")
|
|
114
116
|
|
|
@@ -43,7 +43,9 @@ class Distribution(ABC):
|
|
|
43
43
|
pass
|
|
44
44
|
|
|
45
45
|
@abstractmethod
|
|
46
|
-
def sample(
|
|
46
|
+
def sample(
|
|
47
|
+
self, size: Optional[Union[int, Tuple[int, ...]]] = None
|
|
48
|
+
) -> NDArray[np.floating]:
|
|
47
49
|
"""Generate random samples."""
|
|
48
50
|
pass
|
|
49
51
|
|
|
@@ -102,7 +104,9 @@ class Gaussian(Distribution):
|
|
|
102
104
|
def ppf(self, q: ArrayLike) -> NDArray[np.floating]:
|
|
103
105
|
return np.asarray(self._dist.ppf(q), dtype=np.float64)
|
|
104
106
|
|
|
105
|
-
def sample(
|
|
107
|
+
def sample(
|
|
108
|
+
self, size: Optional[Union[int, Tuple[int, ...]]] = None
|
|
109
|
+
) -> NDArray[np.floating]:
|
|
106
110
|
return np.asarray(self._dist.rvs(size=size), dtype=np.float64)
|
|
107
111
|
|
|
108
112
|
def mean(self) -> float:
|
|
@@ -163,7 +167,9 @@ class MultivariateGaussian(Distribution):
|
|
|
163
167
|
def ppf(self, q: ArrayLike) -> NDArray[np.floating]:
|
|
164
168
|
raise NotImplementedError("PPF not available for multivariate normal")
|
|
165
169
|
|
|
166
|
-
def sample(
|
|
170
|
+
def sample(
|
|
171
|
+
self, size: Optional[Union[int, Tuple[int, ...]]] = None
|
|
172
|
+
) -> NDArray[np.floating]:
|
|
167
173
|
return np.asarray(self._dist.rvs(size=size), dtype=np.float64)
|
|
168
174
|
|
|
169
175
|
def mean(self) -> NDArray[np.floating]:
|
|
@@ -233,7 +239,9 @@ class Uniform(Distribution):
|
|
|
233
239
|
def ppf(self, q: ArrayLike) -> NDArray[np.floating]:
|
|
234
240
|
return np.asarray(self._dist.ppf(q), dtype=np.float64)
|
|
235
241
|
|
|
236
|
-
def sample(
|
|
242
|
+
def sample(
|
|
243
|
+
self, size: Optional[Union[int, Tuple[int, ...]]] = None
|
|
244
|
+
) -> NDArray[np.floating]:
|
|
237
245
|
return np.asarray(self._dist.rvs(size=size), dtype=np.float64)
|
|
238
246
|
|
|
239
247
|
def mean(self) -> float:
|
|
@@ -271,7 +279,9 @@ class Exponential(Distribution):
|
|
|
271
279
|
def ppf(self, q: ArrayLike) -> NDArray[np.floating]:
|
|
272
280
|
return np.asarray(self._dist.ppf(q), dtype=np.float64)
|
|
273
281
|
|
|
274
|
-
def sample(
|
|
282
|
+
def sample(
|
|
283
|
+
self, size: Optional[Union[int, Tuple[int, ...]]] = None
|
|
284
|
+
) -> NDArray[np.floating]:
|
|
275
285
|
return np.asarray(self._dist.rvs(size=size), dtype=np.float64)
|
|
276
286
|
|
|
277
287
|
def mean(self) -> float:
|
|
@@ -337,7 +347,9 @@ class Gamma(Distribution):
|
|
|
337
347
|
def ppf(self, q: ArrayLike) -> NDArray[np.floating]:
|
|
338
348
|
return np.asarray(self._dist.ppf(q), dtype=np.float64)
|
|
339
349
|
|
|
340
|
-
def sample(
|
|
350
|
+
def sample(
|
|
351
|
+
self, size: Optional[Union[int, Tuple[int, ...]]] = None
|
|
352
|
+
) -> NDArray[np.floating]:
|
|
341
353
|
return np.asarray(self._dist.rvs(size=size), dtype=np.float64)
|
|
342
354
|
|
|
343
355
|
def mean(self) -> float:
|
|
@@ -375,7 +387,9 @@ class ChiSquared(Distribution):
|
|
|
375
387
|
def ppf(self, q: ArrayLike) -> NDArray[np.floating]:
|
|
376
388
|
return np.asarray(self._dist.ppf(q), dtype=np.float64)
|
|
377
389
|
|
|
378
|
-
def sample(
|
|
390
|
+
def sample(
|
|
391
|
+
self, size: Optional[Union[int, Tuple[int, ...]]] = None
|
|
392
|
+
) -> NDArray[np.floating]:
|
|
379
393
|
return np.asarray(self._dist.rvs(size=size), dtype=np.float64)
|
|
380
394
|
|
|
381
395
|
def mean(self) -> float:
|
|
@@ -422,7 +436,9 @@ class StudentT(Distribution):
|
|
|
422
436
|
def ppf(self, q: ArrayLike) -> NDArray[np.floating]:
|
|
423
437
|
return np.asarray(self._dist.ppf(q), dtype=np.float64)
|
|
424
438
|
|
|
425
|
-
def sample(
|
|
439
|
+
def sample(
|
|
440
|
+
self, size: Optional[Union[int, Tuple[int, ...]]] = None
|
|
441
|
+
) -> NDArray[np.floating]:
|
|
426
442
|
return np.asarray(self._dist.rvs(size=size), dtype=np.float64)
|
|
427
443
|
|
|
428
444
|
def mean(self) -> float:
|
|
@@ -469,7 +485,9 @@ class Beta(Distribution):
|
|
|
469
485
|
def ppf(self, q: ArrayLike) -> NDArray[np.floating]:
|
|
470
486
|
return np.asarray(self._dist.ppf(q), dtype=np.float64)
|
|
471
487
|
|
|
472
|
-
def sample(
|
|
488
|
+
def sample(
|
|
489
|
+
self, size: Optional[Union[int, Tuple[int, ...]]] = None
|
|
490
|
+
) -> NDArray[np.floating]:
|
|
473
491
|
return np.asarray(self._dist.rvs(size=size), dtype=np.float64)
|
|
474
492
|
|
|
475
493
|
def mean(self) -> float:
|
|
@@ -510,7 +528,9 @@ class Poisson(Distribution):
|
|
|
510
528
|
def ppf(self, q: ArrayLike) -> NDArray[np.floating]:
|
|
511
529
|
return np.asarray(self._dist.ppf(q), dtype=np.float64)
|
|
512
530
|
|
|
513
|
-
def sample(
|
|
531
|
+
def sample(
|
|
532
|
+
self, size: Optional[Union[int, Tuple[int, ...]]] = None
|
|
533
|
+
) -> NDArray[np.floating]:
|
|
514
534
|
return np.asarray(self._dist.rvs(size=size), dtype=np.float64)
|
|
515
535
|
|
|
516
536
|
def mean(self) -> float:
|
|
@@ -553,7 +573,9 @@ class VonMises(Distribution):
|
|
|
553
573
|
def ppf(self, q: ArrayLike) -> NDArray[np.floating]:
|
|
554
574
|
return np.asarray(self._dist.ppf(q), dtype=np.float64)
|
|
555
575
|
|
|
556
|
-
def sample(
|
|
576
|
+
def sample(
|
|
577
|
+
self, size: Optional[Union[int, Tuple[int, ...]]] = None
|
|
578
|
+
) -> NDArray[np.floating]:
|
|
557
579
|
return np.asarray(self._dist.rvs(size=size), dtype=np.float64)
|
|
558
580
|
|
|
559
581
|
def mean(self) -> float:
|
|
@@ -606,7 +628,9 @@ class Wishart(Distribution):
|
|
|
606
628
|
def ppf(self, q: ArrayLike) -> NDArray[np.floating]:
|
|
607
629
|
raise NotImplementedError("PPF not available for Wishart distribution")
|
|
608
630
|
|
|
609
|
-
def sample(
|
|
631
|
+
def sample(
|
|
632
|
+
self, size: Optional[Union[int, Tuple[int, ...]]] = None
|
|
633
|
+
) -> NDArray[np.floating]:
|
|
610
634
|
return np.asarray(self._dist.rvs(size=size), dtype=np.float64)
|
|
611
635
|
|
|
612
636
|
def mean(self) -> NDArray[np.floating]:
|
|
@@ -364,7 +364,9 @@ def kurtosis(
|
|
|
364
364
|
"""
|
|
365
365
|
from scipy.stats import kurtosis as scipy_kurtosis
|
|
366
366
|
|
|
367
|
-
return np.asarray(
|
|
367
|
+
return np.asarray(
|
|
368
|
+
scipy_kurtosis(x, axis=axis, fisher=fisher, bias=bias), dtype=np.float64
|
|
369
|
+
)
|
|
368
370
|
|
|
369
371
|
|
|
370
372
|
def moment(
|
|
@@ -503,8 +503,12 @@ def reassigned_spectrogram(
|
|
|
503
503
|
win_d = np.gradient(win)
|
|
504
504
|
|
|
505
505
|
# STFT with modified windows
|
|
506
|
-
result_t = stft(
|
|
507
|
-
|
|
506
|
+
result_t = stft(
|
|
507
|
+
x, fs=fs, window=win_t, nperseg=nperseg, noverlap=noverlap, nfft=nfft
|
|
508
|
+
)
|
|
509
|
+
result_d = stft(
|
|
510
|
+
x, fs=fs, window=win_d, nperseg=nperseg, noverlap=noverlap, nfft=nfft
|
|
511
|
+
)
|
|
508
512
|
|
|
509
513
|
# Compute reassigned coordinates
|
|
510
514
|
Zxx = result1.Zxx
|
|
@@ -588,7 +592,9 @@ def mel_spectrogram(
|
|
|
588
592
|
noverlap = nperseg // 4
|
|
589
593
|
|
|
590
594
|
# Compute linear spectrogram
|
|
591
|
-
spec_result = spectrogram(
|
|
595
|
+
spec_result = spectrogram(
|
|
596
|
+
x, fs=fs, window=window, nperseg=nperseg, noverlap=noverlap
|
|
597
|
+
)
|
|
592
598
|
|
|
593
599
|
# Create mel filterbank
|
|
594
600
|
mel_fb = _mel_filterbank(
|
|
@@ -526,7 +526,9 @@ def dwt(
|
|
|
526
526
|
- 'biorN.M': Biorthogonal wavelets
|
|
527
527
|
"""
|
|
528
528
|
if not PYWT_AVAILABLE:
|
|
529
|
-
raise ImportError(
|
|
529
|
+
raise ImportError(
|
|
530
|
+
"pywavelets is required for DWT. Install with: pip install pywavelets"
|
|
531
|
+
)
|
|
530
532
|
|
|
531
533
|
signal = np.asarray(signal, dtype=np.float64)
|
|
532
534
|
|
|
@@ -577,7 +579,9 @@ def idwt(
|
|
|
577
579
|
True
|
|
578
580
|
"""
|
|
579
581
|
if not PYWT_AVAILABLE:
|
|
580
|
-
raise ImportError(
|
|
582
|
+
raise ImportError(
|
|
583
|
+
"pywavelets is required for IDWT. Install with: pip install pywavelets"
|
|
584
|
+
)
|
|
581
585
|
|
|
582
586
|
# Reconstruct coeffs list in pywt format
|
|
583
587
|
# [cA_n, cD_n, cD_n-1, ..., cD_1]
|
|
@@ -613,7 +617,9 @@ def dwt_single_level(
|
|
|
613
617
|
Detail coefficients.
|
|
614
618
|
"""
|
|
615
619
|
if not PYWT_AVAILABLE:
|
|
616
|
-
raise ImportError(
|
|
620
|
+
raise ImportError(
|
|
621
|
+
"pywavelets is required for DWT. Install with: pip install pywavelets"
|
|
622
|
+
)
|
|
617
623
|
|
|
618
624
|
signal = np.asarray(signal, dtype=np.float64)
|
|
619
625
|
cA, cD = pywt.dwt(signal, wavelet, mode=mode)
|
|
@@ -647,7 +653,9 @@ def idwt_single_level(
|
|
|
647
653
|
Reconstructed signal.
|
|
648
654
|
"""
|
|
649
655
|
if not PYWT_AVAILABLE:
|
|
650
|
-
raise ImportError(
|
|
656
|
+
raise ImportError(
|
|
657
|
+
"pywavelets is required for IDWT. Install with: pip install pywavelets"
|
|
658
|
+
)
|
|
651
659
|
|
|
652
660
|
cA = np.asarray(cA, dtype=np.float64)
|
|
653
661
|
cD = np.asarray(cD, dtype=np.float64)
|
|
@@ -701,7 +709,9 @@ def wpt(
|
|
|
701
709
|
True
|
|
702
710
|
"""
|
|
703
711
|
if not PYWT_AVAILABLE:
|
|
704
|
-
raise ImportError(
|
|
712
|
+
raise ImportError(
|
|
713
|
+
"pywavelets is required for WPT. Install with: pip install pywavelets"
|
|
714
|
+
)
|
|
705
715
|
|
|
706
716
|
signal = np.asarray(signal, dtype=np.float64)
|
|
707
717
|
|
|
@@ -807,7 +817,9 @@ def threshold_coefficients(
|
|
|
807
817
|
Thresholded coefficients.
|
|
808
818
|
"""
|
|
809
819
|
if not PYWT_AVAILABLE:
|
|
810
|
-
raise ImportError(
|
|
820
|
+
raise ImportError(
|
|
821
|
+
"pywavelets is required. Install with: pip install pywavelets"
|
|
822
|
+
)
|
|
811
823
|
|
|
812
824
|
# Estimate noise from finest detail coefficients
|
|
813
825
|
if value is None:
|
pytcl/navigation/geodesy.py
CHANGED
|
@@ -356,7 +356,9 @@ def ned_to_ecef(
|
|
|
356
356
|
x, y, z : ndarray
|
|
357
357
|
ECEF coordinates in meters.
|
|
358
358
|
"""
|
|
359
|
-
return enu_to_ecef(
|
|
359
|
+
return enu_to_ecef(
|
|
360
|
+
east, north, -np.asarray(down), lat_ref, lon_ref, alt_ref, ellipsoid
|
|
361
|
+
)
|
|
360
362
|
|
|
361
363
|
|
|
362
364
|
def direct_geodetic(
|
|
@@ -434,7 +436,11 @@ def direct_geodetic(
|
|
|
434
436
|
/ 4
|
|
435
437
|
* (
|
|
436
438
|
cos_sigma * (-1 + 2 * cos_2sigma_m**2)
|
|
437
|
-
- B
|
|
439
|
+
- B
|
|
440
|
+
/ 6
|
|
441
|
+
* cos_2sigma_m
|
|
442
|
+
* (-3 + 4 * sin_sigma**2)
|
|
443
|
+
* (-3 + 4 * cos_2sigma_m**2)
|
|
438
444
|
)
|
|
439
445
|
)
|
|
440
446
|
)
|
|
@@ -452,20 +458,27 @@ def direct_geodetic(
|
|
|
452
458
|
lat2 = np.arctan2(
|
|
453
459
|
sin_U2,
|
|
454
460
|
(1 - f)
|
|
455
|
-
* np.sqrt(
|
|
461
|
+
* np.sqrt(
|
|
462
|
+
sin_alpha**2 + (sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_alpha1) ** 2
|
|
463
|
+
),
|
|
456
464
|
)
|
|
457
465
|
|
|
458
|
-
lam = np.arctan2(
|
|
466
|
+
lam = np.arctan2(
|
|
467
|
+
sin_sigma * sin_alpha1, cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_alpha1
|
|
468
|
+
)
|
|
459
469
|
|
|
460
470
|
C = f / 16 * cos2_alpha * (4 + f * (4 - 3 * cos2_alpha))
|
|
461
471
|
L = lam - (1 - C) * f * sin_alpha * (
|
|
462
|
-
sigma
|
|
472
|
+
sigma
|
|
473
|
+
+ C * sin_sigma * (cos_2sigma_m + C * cos_sigma * (-1 + 2 * cos_2sigma_m**2))
|
|
463
474
|
)
|
|
464
475
|
|
|
465
476
|
lon2 = lon1 + L
|
|
466
477
|
|
|
467
478
|
# Back azimuth
|
|
468
|
-
azimuth2 = np.arctan2(
|
|
479
|
+
azimuth2 = np.arctan2(
|
|
480
|
+
sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_alpha1
|
|
481
|
+
)
|
|
469
482
|
|
|
470
483
|
return float(lat2), float(lon2), float(azimuth2)
|
|
471
484
|
|
|
@@ -552,7 +565,10 @@ def inverse_geodetic(
|
|
|
552
565
|
C = f / 16 * cos2_alpha * (4 + f * (4 - 3 * cos2_alpha))
|
|
553
566
|
|
|
554
567
|
lam_new = L + (1 - C) * f * sin_alpha * (
|
|
555
|
-
sigma
|
|
568
|
+
sigma
|
|
569
|
+
+ C
|
|
570
|
+
* sin_sigma
|
|
571
|
+
* (cos_2sigma_m + C * cos_sigma * (-1 + 2 * cos_2sigma_m**2))
|
|
556
572
|
)
|
|
557
573
|
|
|
558
574
|
if abs(lam_new - lam) < 1e-12:
|
|
@@ -572,7 +588,11 @@ def inverse_geodetic(
|
|
|
572
588
|
/ 4
|
|
573
589
|
* (
|
|
574
590
|
cos_sigma * (-1 + 2 * cos_2sigma_m**2)
|
|
575
|
-
- B
|
|
591
|
+
- B
|
|
592
|
+
/ 6
|
|
593
|
+
* cos_2sigma_m
|
|
594
|
+
* (-3 + 4 * sin_sigma**2)
|
|
595
|
+
* (-3 + 4 * cos_2sigma_m**2)
|
|
576
596
|
)
|
|
577
597
|
)
|
|
578
598
|
)
|
|
@@ -581,7 +601,9 @@ def inverse_geodetic(
|
|
|
581
601
|
|
|
582
602
|
# Azimuths
|
|
583
603
|
azimuth1 = np.arctan2(cos_U2 * sin_lam, cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lam)
|
|
584
|
-
azimuth2 = np.arctan2(
|
|
604
|
+
azimuth2 = np.arctan2(
|
|
605
|
+
cos_U1 * sin_lam, -sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lam
|
|
606
|
+
)
|
|
585
607
|
|
|
586
608
|
return float(distance), float(azimuth1), float(azimuth2)
|
|
587
609
|
|
pytcl/navigation/great_circle.py
CHANGED
|
@@ -344,7 +344,9 @@ def great_circle_direct(
|
|
|
344
344
|
"""
|
|
345
345
|
d = distance / radius # Angular distance
|
|
346
346
|
|
|
347
|
-
lat2 = np.arcsin(
|
|
347
|
+
lat2 = np.arcsin(
|
|
348
|
+
np.sin(lat1) * np.cos(d) + np.cos(lat1) * np.sin(d) * np.cos(azimuth)
|
|
349
|
+
)
|
|
348
350
|
|
|
349
351
|
lon2 = lon1 + np.arctan2(
|
|
350
352
|
np.sin(azimuth) * np.sin(d) * np.cos(lat1),
|
|
@@ -406,7 +408,9 @@ def cross_track_distance(
|
|
|
406
408
|
# Along-track distance
|
|
407
409
|
dat = np.arccos(np.cos(d13) / np.cos(dxt))
|
|
408
410
|
|
|
409
|
-
return CrossTrackResult(
|
|
411
|
+
return CrossTrackResult(
|
|
412
|
+
cross_track=float(dxt * radius), along_track=float(dat * radius)
|
|
413
|
+
)
|
|
410
414
|
|
|
411
415
|
|
|
412
416
|
def great_circle_intersect(
|
|
@@ -448,7 +452,9 @@ def great_circle_intersect(
|
|
|
448
452
|
|
|
449
453
|
# Convert to Cartesian unit vectors
|
|
450
454
|
def to_cartesian(lat, lon):
|
|
451
|
-
return np.array(
|
|
455
|
+
return np.array(
|
|
456
|
+
[np.cos(lat) * np.cos(lon), np.cos(lat) * np.sin(lon), np.sin(lat)]
|
|
457
|
+
)
|
|
452
458
|
|
|
453
459
|
# Normal vectors to the great circles
|
|
454
460
|
p1 = to_cartesian(lat1, lon1)
|
|
@@ -502,7 +508,9 @@ def great_circle_intersect(
|
|
|
502
508
|
lat_i2 = -lat_i1
|
|
503
509
|
lon_i2 = ((lon_i1 + np.pi) % (2 * np.pi)) - np.pi
|
|
504
510
|
|
|
505
|
-
return IntersectionResult(
|
|
511
|
+
return IntersectionResult(
|
|
512
|
+
float(lat_i1), float(lon_i1), float(lat_i2), float(lon_i2), True
|
|
513
|
+
)
|
|
506
514
|
|
|
507
515
|
|
|
508
516
|
def great_circle_path_intersect(
|
pytcl/navigation/ins.py
CHANGED
|
@@ -244,7 +244,11 @@ def normal_gravity(lat: float, alt: float = 0.0) -> float:
|
|
|
244
244
|
|
|
245
245
|
# Free-air correction (first-order)
|
|
246
246
|
g = g0 * (
|
|
247
|
-
1
|
|
247
|
+
1
|
|
248
|
+
- 2
|
|
249
|
+
* alt
|
|
250
|
+
/ A_EARTH
|
|
251
|
+
* (1 + F_EARTH + (OMEGA_EARTH**2 * A_EARTH**2 * B_EARTH) / GM_EARTH)
|
|
248
252
|
)
|
|
249
253
|
|
|
250
254
|
return g
|
|
@@ -568,7 +572,9 @@ def update_quaternion(
|
|
|
568
572
|
|
|
569
573
|
if angle < 1e-10:
|
|
570
574
|
# Small angle approximation
|
|
571
|
-
delta_q = np.array(
|
|
575
|
+
delta_q = np.array(
|
|
576
|
+
[1.0, 0.5 * delta_theta[0], 0.5 * delta_theta[1], 0.5 * delta_theta[2]]
|
|
577
|
+
)
|
|
572
578
|
else:
|
|
573
579
|
# Exact quaternion for rotation
|
|
574
580
|
axis = delta_theta / angle
|
pytcl/navigation/ins_gnss.py
CHANGED
|
@@ -512,7 +512,9 @@ def loose_coupled_predict(
|
|
|
512
512
|
dt = imu.dt
|
|
513
513
|
|
|
514
514
|
# Propagate INS mechanization
|
|
515
|
-
ins_new = mechanize_ins_ned(
|
|
515
|
+
ins_new = mechanize_ins_ned(
|
|
516
|
+
state.ins_state, imu, accel_prev=accel_prev, gyro_prev=gyro_prev
|
|
517
|
+
)
|
|
516
518
|
|
|
517
519
|
# Get error state transition matrix (continuous-time)
|
|
518
520
|
F_cont = ins_error_state_matrix(state.ins_state)
|
|
@@ -574,7 +576,9 @@ def loose_coupled_update_position(
|
|
|
574
576
|
if gnss.position_cov is not None:
|
|
575
577
|
R = gnss.position_cov
|
|
576
578
|
else:
|
|
577
|
-
R = np.diag(
|
|
579
|
+
R = np.diag(
|
|
580
|
+
[10.0**2, 10.0**2, 15.0**2]
|
|
581
|
+
) # Default: 10m horizontal, 15m vertical
|
|
578
582
|
|
|
579
583
|
# Innovation: measured position - INS predicted position
|
|
580
584
|
z = gnss.position - state.ins_state.position
|
|
@@ -691,8 +695,16 @@ def loose_coupled_update(
|
|
|
691
695
|
# Full position + velocity update
|
|
692
696
|
H = position_velocity_measurement_matrix()
|
|
693
697
|
|
|
694
|
-
R_pos =
|
|
695
|
-
|
|
698
|
+
R_pos = (
|
|
699
|
+
gnss.position_cov
|
|
700
|
+
if gnss.position_cov is not None
|
|
701
|
+
else np.diag([10.0**2] * 3)
|
|
702
|
+
)
|
|
703
|
+
R_vel = (
|
|
704
|
+
gnss.velocity_cov
|
|
705
|
+
if gnss.velocity_cov is not None
|
|
706
|
+
else np.diag([0.1**2] * 3)
|
|
707
|
+
)
|
|
696
708
|
R = np.block([[R_pos, np.zeros((3, 3))], [np.zeros((3, 3)), R_vel]])
|
|
697
709
|
|
|
698
710
|
z = np.concatenate(
|
|
@@ -834,7 +846,9 @@ def tight_coupled_measurement_matrix(
|
|
|
834
846
|
los, _ = compute_line_of_sight(user_ecef, sat.position)
|
|
835
847
|
|
|
836
848
|
# LOS components in ECEF
|
|
837
|
-
los_x, los_y, los_z =
|
|
849
|
+
los_x, los_y, los_z = (
|
|
850
|
+
-los
|
|
851
|
+
) # Negative because increase in user pos decreases range
|
|
838
852
|
|
|
839
853
|
# Transform LOS to geodetic derivatives
|
|
840
854
|
# d(range)/d(lat), d(range)/d(lon), d(range)/d(alt)
|
|
@@ -844,7 +858,9 @@ def tight_coupled_measurement_matrix(
|
|
|
844
858
|
+ los_z * (cos_lat * N * (1 - ellipsoid.e2))
|
|
845
859
|
)
|
|
846
860
|
H[i, 1] = los_x * (-cos_lat * sin_lon * N) + los_y * (cos_lat * cos_lon * N)
|
|
847
|
-
H[i, 2] =
|
|
861
|
+
H[i, 2] = (
|
|
862
|
+
los_x * cos_lat * cos_lon + los_y * cos_lat * sin_lon + los_z * sin_lat
|
|
863
|
+
)
|
|
848
864
|
|
|
849
865
|
# Clock bias (state 15)
|
|
850
866
|
H[i, 15] = 1.0
|
|
@@ -975,7 +991,9 @@ def _apply_error_correction(
|
|
|
975
991
|
|
|
976
992
|
# Apply small angle rotation to quaternion
|
|
977
993
|
q = ins_state.quaternion
|
|
978
|
-
delta_q = np.array(
|
|
994
|
+
delta_q = np.array(
|
|
995
|
+
[1.0, 0.5 * phi[0], 0.5 * phi[1], 0.5 * phi[2]], dtype=np.float64
|
|
996
|
+
)
|
|
979
997
|
delta_q = delta_q / np.linalg.norm(delta_q)
|
|
980
998
|
|
|
981
999
|
# Quaternion multiplication (body frame correction)
|
pytcl/navigation/rhumb.py
CHANGED
|
@@ -100,7 +100,9 @@ def _isometric_latitude(lat: float, e2: float = 0.0) -> float:
|
|
|
100
100
|
)
|
|
101
101
|
|
|
102
102
|
|
|
103
|
-
def _inverse_isometric_latitude(
|
|
103
|
+
def _inverse_isometric_latitude(
|
|
104
|
+
psi: float, e2: float = 0.0, max_iter: int = 20
|
|
105
|
+
) -> float:
|
|
104
106
|
"""
|
|
105
107
|
Compute geodetic latitude from isometric latitude.
|
|
106
108
|
|
|
@@ -129,7 +131,10 @@ def _inverse_isometric_latitude(psi: float, e2: float = 0.0, max_iter: int = 20)
|
|
|
129
131
|
for _ in range(max_iter):
|
|
130
132
|
sin_lat = np.sin(lat)
|
|
131
133
|
lat_new = (
|
|
132
|
-
2
|
|
134
|
+
2
|
|
135
|
+
* np.arctan(
|
|
136
|
+
((1 + e * sin_lat) / (1 - e * sin_lat)) ** (e / 2) * np.exp(psi)
|
|
137
|
+
)
|
|
133
138
|
- np.pi / 2
|
|
134
139
|
)
|
|
135
140
|
if abs(lat_new - lat) < 1e-12:
|
|
@@ -422,7 +427,9 @@ def indirect_rhumb(
|
|
|
422
427
|
if abs(dlon) > np.pi:
|
|
423
428
|
dlon = dlon - np.sign(dlon) * 2 * np.pi
|
|
424
429
|
|
|
425
|
-
dpsi = _isometric_latitude(lat2, ellipsoid.e2) - _isometric_latitude(
|
|
430
|
+
dpsi = _isometric_latitude(lat2, ellipsoid.e2) - _isometric_latitude(
|
|
431
|
+
lat1, ellipsoid.e2
|
|
432
|
+
)
|
|
426
433
|
bearing = np.arctan2(dlon, dpsi) % (2 * np.pi)
|
|
427
434
|
|
|
428
435
|
return RhumbResult(distance, bearing)
|
|
@@ -158,7 +158,9 @@ def ospa(
|
|
|
158
158
|
loc_component = (localization_sum / n) ** (1.0 / p) if localization_sum > 0 else 0.0
|
|
159
159
|
card_component = (cardinality_penalty / n) ** (1.0 / p) if n > m else 0.0
|
|
160
160
|
|
|
161
|
-
return OSPAResult(
|
|
161
|
+
return OSPAResult(
|
|
162
|
+
ospa=ospa_val, localization=loc_component, cardinality=card_component
|
|
163
|
+
)
|
|
162
164
|
|
|
163
165
|
|
|
164
166
|
def ospa_over_time(
|
pytcl/plotting/coordinates.py
CHANGED
|
@@ -181,13 +181,19 @@ def plot_euler_angles(
|
|
|
181
181
|
|
|
182
182
|
# Create rotation matrices for each axis
|
|
183
183
|
def rotx(a):
|
|
184
|
-
return np.array(
|
|
184
|
+
return np.array(
|
|
185
|
+
[[1, 0, 0], [0, np.cos(a), -np.sin(a)], [0, np.sin(a), np.cos(a)]]
|
|
186
|
+
)
|
|
185
187
|
|
|
186
188
|
def roty(a):
|
|
187
|
-
return np.array(
|
|
189
|
+
return np.array(
|
|
190
|
+
[[np.cos(a), 0, np.sin(a)], [0, 1, 0], [-np.sin(a), 0, np.cos(a)]]
|
|
191
|
+
)
|
|
188
192
|
|
|
189
193
|
def rotz(a):
|
|
190
|
-
return np.array(
|
|
194
|
+
return np.array(
|
|
195
|
+
[[np.cos(a), -np.sin(a), 0], [np.sin(a), np.cos(a), 0], [0, 0, 1]]
|
|
196
|
+
)
|
|
191
197
|
|
|
192
198
|
rot_funcs = {"X": rotx, "Y": roty, "Z": rotz}
|
|
193
199
|
|
|
@@ -241,7 +247,11 @@ def plot_euler_angles(
|
|
|
241
247
|
for i in range(4):
|
|
242
248
|
scene_name = f"scene{i + 1}" if i > 0 else "scene"
|
|
243
249
|
fig.update_layout(
|
|
244
|
-
**{
|
|
250
|
+
**{
|
|
251
|
+
scene_name: dict(
|
|
252
|
+
aspectmode="cube", camera=dict(eye=dict(x=1.5, y=1.5, z=1.5))
|
|
253
|
+
)
|
|
254
|
+
}
|
|
245
255
|
)
|
|
246
256
|
|
|
247
257
|
return fig
|
|
@@ -368,7 +378,9 @@ def plot_quaternion_interpolation(
|
|
|
368
378
|
method="animate",
|
|
369
379
|
args=[
|
|
370
380
|
[None],
|
|
371
|
-
dict(
|
|
381
|
+
dict(
|
|
382
|
+
frame=dict(duration=0, redraw=False), mode="immediate"
|
|
383
|
+
),
|
|
372
384
|
],
|
|
373
385
|
),
|
|
374
386
|
],
|