nrl-tracker 0.21.4__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.4.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.4.dist-info/RECORD +0 -148
- {nrl_tracker-0.21.4.dist-info → nrl_tracker-0.22.0.dist-info}/LICENSE +0 -0
- {nrl_tracker-0.21.4.dist-info → nrl_tracker-0.22.0.dist-info}/WHEEL +0 -0
- {nrl_tracker-0.21.4.dist-info → nrl_tracker-0.22.0.dist-info}/top_level.txt +0 -0
|
@@ -7,57 +7,51 @@ This module provides signal transforms for time-frequency analysis:
|
|
|
7
7
|
- Wavelet transforms (CWT and DWT)
|
|
8
8
|
"""
|
|
9
9
|
|
|
10
|
-
from pytcl.mathematical_functions.transforms.fourier import
|
|
11
|
-
|
|
12
|
-
|
|
13
|
-
|
|
14
|
-
|
|
15
|
-
|
|
16
|
-
|
|
17
|
-
|
|
18
|
-
|
|
19
|
-
|
|
20
|
-
|
|
21
|
-
|
|
22
|
-
|
|
23
|
-
|
|
24
|
-
|
|
25
|
-
|
|
26
|
-
|
|
27
|
-
|
|
28
|
-
|
|
29
|
-
|
|
30
|
-
|
|
31
|
-
from pytcl.mathematical_functions.transforms.stft import
|
|
32
|
-
|
|
33
|
-
|
|
34
|
-
|
|
35
|
-
|
|
36
|
-
|
|
37
|
-
|
|
38
|
-
|
|
39
|
-
|
|
40
|
-
|
|
41
|
-
|
|
42
|
-
from pytcl.mathematical_functions.transforms.wavelets import
|
|
43
|
-
|
|
44
|
-
|
|
45
|
-
|
|
46
|
-
|
|
47
|
-
|
|
48
|
-
|
|
49
|
-
|
|
50
|
-
|
|
51
|
-
|
|
52
|
-
|
|
53
|
-
|
|
54
|
-
|
|
55
|
-
ricker_wavelet,
|
|
56
|
-
scales_to_frequencies,
|
|
57
|
-
threshold_coefficients,
|
|
58
|
-
wavelet_info,
|
|
59
|
-
wpt,
|
|
60
|
-
)
|
|
10
|
+
from pytcl.mathematical_functions.transforms.fourier import CoherenceResult
|
|
11
|
+
from pytcl.mathematical_functions.transforms.fourier import CrossSpectrum
|
|
12
|
+
from pytcl.mathematical_functions.transforms.fourier import PowerSpectrum
|
|
13
|
+
from pytcl.mathematical_functions.transforms.fourier import coherence
|
|
14
|
+
from pytcl.mathematical_functions.transforms.fourier import cross_spectrum
|
|
15
|
+
from pytcl.mathematical_functions.transforms.fourier import fft
|
|
16
|
+
from pytcl.mathematical_functions.transforms.fourier import fft2
|
|
17
|
+
from pytcl.mathematical_functions.transforms.fourier import fftshift
|
|
18
|
+
from pytcl.mathematical_functions.transforms.fourier import frequency_axis
|
|
19
|
+
from pytcl.mathematical_functions.transforms.fourier import ifft
|
|
20
|
+
from pytcl.mathematical_functions.transforms.fourier import ifft2
|
|
21
|
+
from pytcl.mathematical_functions.transforms.fourier import ifftshift
|
|
22
|
+
from pytcl.mathematical_functions.transforms.fourier import irfft
|
|
23
|
+
from pytcl.mathematical_functions.transforms.fourier import magnitude_spectrum
|
|
24
|
+
from pytcl.mathematical_functions.transforms.fourier import periodogram
|
|
25
|
+
from pytcl.mathematical_functions.transforms.fourier import phase_spectrum
|
|
26
|
+
from pytcl.mathematical_functions.transforms.fourier import power_spectrum
|
|
27
|
+
from pytcl.mathematical_functions.transforms.fourier import rfft
|
|
28
|
+
from pytcl.mathematical_functions.transforms.fourier import rfft_frequency_axis
|
|
29
|
+
from pytcl.mathematical_functions.transforms.stft import Spectrogram
|
|
30
|
+
from pytcl.mathematical_functions.transforms.stft import STFTResult
|
|
31
|
+
from pytcl.mathematical_functions.transforms.stft import get_window
|
|
32
|
+
from pytcl.mathematical_functions.transforms.stft import istft
|
|
33
|
+
from pytcl.mathematical_functions.transforms.stft import mel_spectrogram
|
|
34
|
+
from pytcl.mathematical_functions.transforms.stft import reassigned_spectrogram
|
|
35
|
+
from pytcl.mathematical_functions.transforms.stft import spectrogram
|
|
36
|
+
from pytcl.mathematical_functions.transforms.stft import stft
|
|
37
|
+
from pytcl.mathematical_functions.transforms.stft import window_bandwidth
|
|
38
|
+
from pytcl.mathematical_functions.transforms.wavelets import PYWT_AVAILABLE
|
|
39
|
+
from pytcl.mathematical_functions.transforms.wavelets import CWTResult
|
|
40
|
+
from pytcl.mathematical_functions.transforms.wavelets import DWTResult
|
|
41
|
+
from pytcl.mathematical_functions.transforms.wavelets import available_wavelets
|
|
42
|
+
from pytcl.mathematical_functions.transforms.wavelets import cwt
|
|
43
|
+
from pytcl.mathematical_functions.transforms.wavelets import dwt
|
|
44
|
+
from pytcl.mathematical_functions.transforms.wavelets import dwt_single_level
|
|
45
|
+
from pytcl.mathematical_functions.transforms.wavelets import frequencies_to_scales
|
|
46
|
+
from pytcl.mathematical_functions.transforms.wavelets import gaussian_wavelet
|
|
47
|
+
from pytcl.mathematical_functions.transforms.wavelets import idwt
|
|
48
|
+
from pytcl.mathematical_functions.transforms.wavelets import idwt_single_level
|
|
49
|
+
from pytcl.mathematical_functions.transforms.wavelets import morlet_wavelet
|
|
50
|
+
from pytcl.mathematical_functions.transforms.wavelets import ricker_wavelet
|
|
51
|
+
from pytcl.mathematical_functions.transforms.wavelets import scales_to_frequencies
|
|
52
|
+
from pytcl.mathematical_functions.transforms.wavelets import threshold_coefficients
|
|
53
|
+
from pytcl.mathematical_functions.transforms.wavelets import wavelet_info
|
|
54
|
+
from pytcl.mathematical_functions.transforms.wavelets import wpt
|
|
61
55
|
|
|
62
56
|
__all__ = [
|
|
63
57
|
# Fourier transform types
|
|
@@ -23,10 +23,13 @@ References
|
|
|
23
23
|
Electroacoustics, 15(2), 70-73.
|
|
24
24
|
"""
|
|
25
25
|
|
|
26
|
-
from typing import NamedTuple
|
|
26
|
+
from typing import NamedTuple
|
|
27
|
+
from typing import Optional
|
|
28
|
+
from typing import Union
|
|
27
29
|
|
|
28
30
|
import numpy as np
|
|
29
|
-
from numpy.typing import ArrayLike
|
|
31
|
+
from numpy.typing import ArrayLike
|
|
32
|
+
from numpy.typing import NDArray
|
|
30
33
|
from scipy import fft as scipy_fft
|
|
31
34
|
from scipy import signal as scipy_signal
|
|
32
35
|
|
|
@@ -22,10 +22,13 @@ References
|
|
|
22
22
|
Speech, and Signal Processing, 32(2), 236-243.
|
|
23
23
|
"""
|
|
24
24
|
|
|
25
|
-
from typing import NamedTuple
|
|
25
|
+
from typing import NamedTuple
|
|
26
|
+
from typing import Optional
|
|
27
|
+
from typing import Union
|
|
26
28
|
|
|
27
29
|
import numpy as np
|
|
28
|
-
from numpy.typing import ArrayLike
|
|
30
|
+
from numpy.typing import ArrayLike
|
|
31
|
+
from numpy.typing import NDArray
|
|
29
32
|
from scipy import signal as scipy_signal
|
|
30
33
|
|
|
31
34
|
# =============================================================================
|
|
@@ -503,12 +506,8 @@ def reassigned_spectrogram(
|
|
|
503
506
|
win_d = np.gradient(win)
|
|
504
507
|
|
|
505
508
|
# STFT with modified windows
|
|
506
|
-
result_t = stft(
|
|
507
|
-
|
|
508
|
-
)
|
|
509
|
-
result_d = stft(
|
|
510
|
-
x, fs=fs, window=win_d, nperseg=nperseg, noverlap=noverlap, nfft=nfft
|
|
511
|
-
)
|
|
509
|
+
result_t = stft(x, fs=fs, window=win_t, nperseg=nperseg, noverlap=noverlap, nfft=nfft)
|
|
510
|
+
result_d = stft(x, fs=fs, window=win_d, nperseg=nperseg, noverlap=noverlap, nfft=nfft)
|
|
512
511
|
|
|
513
512
|
# Compute reassigned coordinates
|
|
514
513
|
Zxx = result1.Zxx
|
|
@@ -592,9 +591,7 @@ def mel_spectrogram(
|
|
|
592
591
|
noverlap = nperseg // 4
|
|
593
592
|
|
|
594
593
|
# Compute linear spectrogram
|
|
595
|
-
spec_result = spectrogram(
|
|
596
|
-
x, fs=fs, window=window, nperseg=nperseg, noverlap=noverlap
|
|
597
|
-
)
|
|
594
|
+
spec_result = spectrogram(x, fs=fs, window=window, nperseg=nperseg, noverlap=noverlap)
|
|
598
595
|
|
|
599
596
|
# Create mel filterbank
|
|
600
597
|
mel_fb = _mel_filterbank(
|
|
@@ -20,10 +20,15 @@ References
|
|
|
20
20
|
.. [2] Daubechies, I. (1992). Ten Lectures on Wavelets. SIAM.
|
|
21
21
|
"""
|
|
22
22
|
|
|
23
|
-
from typing import Callable
|
|
23
|
+
from typing import Callable
|
|
24
|
+
from typing import List
|
|
25
|
+
from typing import NamedTuple
|
|
26
|
+
from typing import Optional
|
|
27
|
+
from typing import Union
|
|
24
28
|
|
|
25
29
|
import numpy as np
|
|
26
|
-
from numpy.typing import ArrayLike
|
|
30
|
+
from numpy.typing import ArrayLike
|
|
31
|
+
from numpy.typing import NDArray
|
|
27
32
|
|
|
28
33
|
# Try to import pywavelets for DWT support
|
|
29
34
|
try:
|
|
@@ -526,9 +531,7 @@ def dwt(
|
|
|
526
531
|
- 'biorN.M': Biorthogonal wavelets
|
|
527
532
|
"""
|
|
528
533
|
if not PYWT_AVAILABLE:
|
|
529
|
-
raise ImportError(
|
|
530
|
-
"pywavelets is required for DWT. Install with: pip install pywavelets"
|
|
531
|
-
)
|
|
534
|
+
raise ImportError("pywavelets is required for DWT. Install with: pip install pywavelets")
|
|
532
535
|
|
|
533
536
|
signal = np.asarray(signal, dtype=np.float64)
|
|
534
537
|
|
|
@@ -579,9 +582,7 @@ def idwt(
|
|
|
579
582
|
True
|
|
580
583
|
"""
|
|
581
584
|
if not PYWT_AVAILABLE:
|
|
582
|
-
raise ImportError(
|
|
583
|
-
"pywavelets is required for IDWT. Install with: pip install pywavelets"
|
|
584
|
-
)
|
|
585
|
+
raise ImportError("pywavelets is required for IDWT. Install with: pip install pywavelets")
|
|
585
586
|
|
|
586
587
|
# Reconstruct coeffs list in pywt format
|
|
587
588
|
# [cA_n, cD_n, cD_n-1, ..., cD_1]
|
|
@@ -617,9 +618,7 @@ def dwt_single_level(
|
|
|
617
618
|
Detail coefficients.
|
|
618
619
|
"""
|
|
619
620
|
if not PYWT_AVAILABLE:
|
|
620
|
-
raise ImportError(
|
|
621
|
-
"pywavelets is required for DWT. Install with: pip install pywavelets"
|
|
622
|
-
)
|
|
621
|
+
raise ImportError("pywavelets is required for DWT. Install with: pip install pywavelets")
|
|
623
622
|
|
|
624
623
|
signal = np.asarray(signal, dtype=np.float64)
|
|
625
624
|
cA, cD = pywt.dwt(signal, wavelet, mode=mode)
|
|
@@ -653,9 +652,7 @@ def idwt_single_level(
|
|
|
653
652
|
Reconstructed signal.
|
|
654
653
|
"""
|
|
655
654
|
if not PYWT_AVAILABLE:
|
|
656
|
-
raise ImportError(
|
|
657
|
-
"pywavelets is required for IDWT. Install with: pip install pywavelets"
|
|
658
|
-
)
|
|
655
|
+
raise ImportError("pywavelets is required for IDWT. Install with: pip install pywavelets")
|
|
659
656
|
|
|
660
657
|
cA = np.asarray(cA, dtype=np.float64)
|
|
661
658
|
cD = np.asarray(cD, dtype=np.float64)
|
|
@@ -709,9 +706,7 @@ def wpt(
|
|
|
709
706
|
True
|
|
710
707
|
"""
|
|
711
708
|
if not PYWT_AVAILABLE:
|
|
712
|
-
raise ImportError(
|
|
713
|
-
"pywavelets is required for WPT. Install with: pip install pywavelets"
|
|
714
|
-
)
|
|
709
|
+
raise ImportError("pywavelets is required for WPT. Install with: pip install pywavelets")
|
|
715
710
|
|
|
716
711
|
signal = np.asarray(signal, dtype=np.float64)
|
|
717
712
|
|
|
@@ -817,9 +812,7 @@ def threshold_coefficients(
|
|
|
817
812
|
Thresholded coefficients.
|
|
818
813
|
"""
|
|
819
814
|
if not PYWT_AVAILABLE:
|
|
820
|
-
raise ImportError(
|
|
821
|
-
"pywavelets is required. Install with: pip install pywavelets"
|
|
822
|
-
)
|
|
815
|
+
raise ImportError("pywavelets is required. Install with: pip install pywavelets")
|
|
823
816
|
|
|
824
817
|
# Estimate noise from finest detail coefficients
|
|
825
818
|
if value is None:
|
pytcl/navigation/__init__.py
CHANGED
|
@@ -11,110 +11,104 @@ needed in tracking applications, including:
|
|
|
11
11
|
- Rhumb line navigation
|
|
12
12
|
"""
|
|
13
13
|
|
|
14
|
-
from pytcl.navigation.geodesy import (
|
|
15
|
-
GRS80,
|
|
16
|
-
SPHERE,
|
|
17
|
-
WGS84,
|
|
18
|
-
Ellipsoid,
|
|
19
|
-
direct_geodetic,
|
|
20
|
-
ecef_to_enu,
|
|
21
|
-
ecef_to_geodetic,
|
|
22
|
-
ecef_to_ned,
|
|
23
|
-
enu_to_ecef,
|
|
24
|
-
geodetic_to_ecef,
|
|
25
|
-
haversine_distance,
|
|
26
|
-
inverse_geodetic,
|
|
27
|
-
ned_to_ecef,
|
|
14
|
+
from pytcl.navigation.geodesy import (
|
|
15
|
+
GRS80, # Ellipsoids; Coordinate conversions; Geodetic problems
|
|
28
16
|
)
|
|
29
|
-
from pytcl.navigation.
|
|
30
|
-
|
|
31
|
-
|
|
32
|
-
|
|
33
|
-
|
|
34
|
-
|
|
35
|
-
|
|
36
|
-
|
|
37
|
-
|
|
38
|
-
|
|
39
|
-
|
|
40
|
-
|
|
41
|
-
|
|
42
|
-
|
|
43
|
-
|
|
44
|
-
|
|
45
|
-
|
|
46
|
-
|
|
47
|
-
|
|
48
|
-
from pytcl.navigation.
|
|
49
|
-
|
|
50
|
-
|
|
51
|
-
|
|
52
|
-
|
|
53
|
-
|
|
54
|
-
|
|
55
|
-
|
|
56
|
-
|
|
57
|
-
|
|
58
|
-
|
|
59
|
-
|
|
60
|
-
coning_correction,
|
|
61
|
-
earth_rate_ned,
|
|
62
|
-
gravity_ned,
|
|
63
|
-
gyrocompass_alignment,
|
|
64
|
-
initialize_ins_state,
|
|
65
|
-
ins_error_state_matrix,
|
|
66
|
-
ins_process_noise_matrix,
|
|
67
|
-
mechanize_ins_ned,
|
|
68
|
-
normal_gravity,
|
|
69
|
-
radii_of_curvature,
|
|
70
|
-
sculling_correction,
|
|
71
|
-
skew_symmetric,
|
|
72
|
-
transport_rate_ned,
|
|
73
|
-
update_attitude_ned,
|
|
74
|
-
update_quaternion,
|
|
75
|
-
)
|
|
76
|
-
from pytcl.navigation.ins_gnss import ( # INS/GNSS integration
|
|
77
|
-
GPS_L1_FREQ,
|
|
78
|
-
GPS_L1_WAVELENGTH,
|
|
79
|
-
SPEED_OF_LIGHT,
|
|
80
|
-
GNSSMeasurement,
|
|
81
|
-
INSGNSSState,
|
|
82
|
-
LooseCoupledResult,
|
|
83
|
-
SatelliteInfo,
|
|
84
|
-
TightCoupledResult,
|
|
85
|
-
compute_dop,
|
|
86
|
-
compute_line_of_sight,
|
|
87
|
-
gnss_outage_detection,
|
|
88
|
-
initialize_ins_gnss,
|
|
89
|
-
loose_coupled_predict,
|
|
90
|
-
loose_coupled_update,
|
|
91
|
-
loose_coupled_update_position,
|
|
92
|
-
loose_coupled_update_velocity,
|
|
93
|
-
position_measurement_matrix,
|
|
94
|
-
position_velocity_measurement_matrix,
|
|
95
|
-
pseudorange_measurement_matrix,
|
|
96
|
-
satellite_elevation_azimuth,
|
|
97
|
-
tight_coupled_measurement_matrix,
|
|
98
|
-
tight_coupled_pseudorange_innovation,
|
|
99
|
-
tight_coupled_update,
|
|
100
|
-
velocity_measurement_matrix,
|
|
101
|
-
)
|
|
102
|
-
from pytcl.navigation.rhumb import ( # Rhumb line navigation
|
|
103
|
-
RhumbDirectResult,
|
|
104
|
-
RhumbIntersectionResult,
|
|
105
|
-
RhumbResult,
|
|
106
|
-
compare_great_circle_rhumb,
|
|
107
|
-
direct_rhumb,
|
|
108
|
-
direct_rhumb_spherical,
|
|
109
|
-
indirect_rhumb,
|
|
110
|
-
indirect_rhumb_spherical,
|
|
111
|
-
rhumb_bearing,
|
|
112
|
-
rhumb_distance_ellipsoidal,
|
|
113
|
-
rhumb_distance_spherical,
|
|
114
|
-
rhumb_intersect,
|
|
115
|
-
rhumb_midpoint,
|
|
116
|
-
rhumb_waypoints,
|
|
17
|
+
from pytcl.navigation.geodesy import SPHERE
|
|
18
|
+
from pytcl.navigation.geodesy import WGS84
|
|
19
|
+
from pytcl.navigation.geodesy import Ellipsoid
|
|
20
|
+
from pytcl.navigation.geodesy import direct_geodetic
|
|
21
|
+
from pytcl.navigation.geodesy import ecef_to_enu
|
|
22
|
+
from pytcl.navigation.geodesy import ecef_to_geodetic
|
|
23
|
+
from pytcl.navigation.geodesy import ecef_to_ned
|
|
24
|
+
from pytcl.navigation.geodesy import enu_to_ecef
|
|
25
|
+
from pytcl.navigation.geodesy import geodetic_to_ecef
|
|
26
|
+
from pytcl.navigation.geodesy import haversine_distance
|
|
27
|
+
from pytcl.navigation.geodesy import inverse_geodetic
|
|
28
|
+
from pytcl.navigation.geodesy import ned_to_ecef
|
|
29
|
+
from pytcl.navigation.great_circle import EARTH_RADIUS # Great circle navigation
|
|
30
|
+
from pytcl.navigation.great_circle import CrossTrackResult
|
|
31
|
+
from pytcl.navigation.great_circle import GreatCircleResult
|
|
32
|
+
from pytcl.navigation.great_circle import IntersectionResult
|
|
33
|
+
from pytcl.navigation.great_circle import WaypointResult
|
|
34
|
+
from pytcl.navigation.great_circle import angular_distance
|
|
35
|
+
from pytcl.navigation.great_circle import cross_track_distance
|
|
36
|
+
from pytcl.navigation.great_circle import destination_point
|
|
37
|
+
from pytcl.navigation.great_circle import great_circle_azimuth
|
|
38
|
+
from pytcl.navigation.great_circle import great_circle_direct
|
|
39
|
+
from pytcl.navigation.great_circle import great_circle_distance
|
|
40
|
+
from pytcl.navigation.great_circle import great_circle_intersect
|
|
41
|
+
from pytcl.navigation.great_circle import great_circle_inverse
|
|
42
|
+
from pytcl.navigation.great_circle import great_circle_path_intersect
|
|
43
|
+
from pytcl.navigation.great_circle import great_circle_tdoa_loc
|
|
44
|
+
from pytcl.navigation.great_circle import great_circle_waypoint
|
|
45
|
+
from pytcl.navigation.great_circle import great_circle_waypoints
|
|
46
|
+
from pytcl.navigation.ins import (
|
|
47
|
+
A_EARTH, # Constants; State representation; Gravity and Earth rate
|
|
117
48
|
)
|
|
49
|
+
from pytcl.navigation.ins import B_EARTH
|
|
50
|
+
from pytcl.navigation.ins import E2_EARTH
|
|
51
|
+
from pytcl.navigation.ins import F_EARTH
|
|
52
|
+
from pytcl.navigation.ins import GM_EARTH
|
|
53
|
+
from pytcl.navigation.ins import OMEGA_EARTH
|
|
54
|
+
from pytcl.navigation.ins import IMUData
|
|
55
|
+
from pytcl.navigation.ins import INSErrorState
|
|
56
|
+
from pytcl.navigation.ins import INSState
|
|
57
|
+
from pytcl.navigation.ins import coarse_alignment
|
|
58
|
+
from pytcl.navigation.ins import compensate_imu_data
|
|
59
|
+
from pytcl.navigation.ins import coning_correction
|
|
60
|
+
from pytcl.navigation.ins import earth_rate_ned
|
|
61
|
+
from pytcl.navigation.ins import gravity_ned
|
|
62
|
+
from pytcl.navigation.ins import gyrocompass_alignment
|
|
63
|
+
from pytcl.navigation.ins import initialize_ins_state
|
|
64
|
+
from pytcl.navigation.ins import ins_error_state_matrix
|
|
65
|
+
from pytcl.navigation.ins import ins_process_noise_matrix
|
|
66
|
+
from pytcl.navigation.ins import mechanize_ins_ned
|
|
67
|
+
from pytcl.navigation.ins import normal_gravity
|
|
68
|
+
from pytcl.navigation.ins import radii_of_curvature
|
|
69
|
+
from pytcl.navigation.ins import sculling_correction
|
|
70
|
+
from pytcl.navigation.ins import skew_symmetric
|
|
71
|
+
from pytcl.navigation.ins import transport_rate_ned
|
|
72
|
+
from pytcl.navigation.ins import update_attitude_ned
|
|
73
|
+
from pytcl.navigation.ins import update_quaternion
|
|
74
|
+
from pytcl.navigation.ins_gnss import GPS_L1_FREQ # INS/GNSS integration
|
|
75
|
+
from pytcl.navigation.ins_gnss import GPS_L1_WAVELENGTH
|
|
76
|
+
from pytcl.navigation.ins_gnss import SPEED_OF_LIGHT
|
|
77
|
+
from pytcl.navigation.ins_gnss import GNSSMeasurement
|
|
78
|
+
from pytcl.navigation.ins_gnss import INSGNSSState
|
|
79
|
+
from pytcl.navigation.ins_gnss import LooseCoupledResult
|
|
80
|
+
from pytcl.navigation.ins_gnss import SatelliteInfo
|
|
81
|
+
from pytcl.navigation.ins_gnss import TightCoupledResult
|
|
82
|
+
from pytcl.navigation.ins_gnss import compute_dop
|
|
83
|
+
from pytcl.navigation.ins_gnss import compute_line_of_sight
|
|
84
|
+
from pytcl.navigation.ins_gnss import gnss_outage_detection
|
|
85
|
+
from pytcl.navigation.ins_gnss import initialize_ins_gnss
|
|
86
|
+
from pytcl.navigation.ins_gnss import loose_coupled_predict
|
|
87
|
+
from pytcl.navigation.ins_gnss import loose_coupled_update
|
|
88
|
+
from pytcl.navigation.ins_gnss import loose_coupled_update_position
|
|
89
|
+
from pytcl.navigation.ins_gnss import loose_coupled_update_velocity
|
|
90
|
+
from pytcl.navigation.ins_gnss import position_measurement_matrix
|
|
91
|
+
from pytcl.navigation.ins_gnss import position_velocity_measurement_matrix
|
|
92
|
+
from pytcl.navigation.ins_gnss import pseudorange_measurement_matrix
|
|
93
|
+
from pytcl.navigation.ins_gnss import satellite_elevation_azimuth
|
|
94
|
+
from pytcl.navigation.ins_gnss import tight_coupled_measurement_matrix
|
|
95
|
+
from pytcl.navigation.ins_gnss import tight_coupled_pseudorange_innovation
|
|
96
|
+
from pytcl.navigation.ins_gnss import tight_coupled_update
|
|
97
|
+
from pytcl.navigation.ins_gnss import velocity_measurement_matrix
|
|
98
|
+
from pytcl.navigation.rhumb import RhumbDirectResult # Rhumb line navigation
|
|
99
|
+
from pytcl.navigation.rhumb import RhumbIntersectionResult
|
|
100
|
+
from pytcl.navigation.rhumb import RhumbResult
|
|
101
|
+
from pytcl.navigation.rhumb import compare_great_circle_rhumb
|
|
102
|
+
from pytcl.navigation.rhumb import direct_rhumb
|
|
103
|
+
from pytcl.navigation.rhumb import direct_rhumb_spherical
|
|
104
|
+
from pytcl.navigation.rhumb import indirect_rhumb
|
|
105
|
+
from pytcl.navigation.rhumb import indirect_rhumb_spherical
|
|
106
|
+
from pytcl.navigation.rhumb import rhumb_bearing
|
|
107
|
+
from pytcl.navigation.rhumb import rhumb_distance_ellipsoidal
|
|
108
|
+
from pytcl.navigation.rhumb import rhumb_distance_spherical
|
|
109
|
+
from pytcl.navigation.rhumb import rhumb_intersect
|
|
110
|
+
from pytcl.navigation.rhumb import rhumb_midpoint
|
|
111
|
+
from pytcl.navigation.rhumb import rhumb_waypoints
|
|
118
112
|
|
|
119
113
|
__all__ = [
|
|
120
114
|
# Ellipsoids
|
pytcl/navigation/geodesy.py
CHANGED
|
@@ -8,10 +8,12 @@ This module provides geodetic utilities including:
|
|
|
8
8
|
- Earth ellipsoid parameters
|
|
9
9
|
"""
|
|
10
10
|
|
|
11
|
-
from typing import NamedTuple
|
|
11
|
+
from typing import NamedTuple
|
|
12
|
+
from typing import Tuple
|
|
12
13
|
|
|
13
14
|
import numpy as np
|
|
14
|
-
from numpy.typing import ArrayLike
|
|
15
|
+
from numpy.typing import ArrayLike
|
|
16
|
+
from numpy.typing import NDArray
|
|
15
17
|
|
|
16
18
|
|
|
17
19
|
class Ellipsoid(NamedTuple):
|
|
@@ -356,9 +358,7 @@ def ned_to_ecef(
|
|
|
356
358
|
x, y, z : ndarray
|
|
357
359
|
ECEF coordinates in meters.
|
|
358
360
|
"""
|
|
359
|
-
return enu_to_ecef(
|
|
360
|
-
east, north, -np.asarray(down), lat_ref, lon_ref, alt_ref, ellipsoid
|
|
361
|
-
)
|
|
361
|
+
return enu_to_ecef(east, north, -np.asarray(down), lat_ref, lon_ref, alt_ref, ellipsoid)
|
|
362
362
|
|
|
363
363
|
|
|
364
364
|
def direct_geodetic(
|
|
@@ -436,11 +436,7 @@ def direct_geodetic(
|
|
|
436
436
|
/ 4
|
|
437
437
|
* (
|
|
438
438
|
cos_sigma * (-1 + 2 * cos_2sigma_m**2)
|
|
439
|
-
- B
|
|
440
|
-
/ 6
|
|
441
|
-
* cos_2sigma_m
|
|
442
|
-
* (-3 + 4 * sin_sigma**2)
|
|
443
|
-
* (-3 + 4 * cos_2sigma_m**2)
|
|
439
|
+
- B / 6 * cos_2sigma_m * (-3 + 4 * sin_sigma**2) * (-3 + 4 * cos_2sigma_m**2)
|
|
444
440
|
)
|
|
445
441
|
)
|
|
446
442
|
)
|
|
@@ -458,27 +454,20 @@ def direct_geodetic(
|
|
|
458
454
|
lat2 = np.arctan2(
|
|
459
455
|
sin_U2,
|
|
460
456
|
(1 - f)
|
|
461
|
-
* np.sqrt(
|
|
462
|
-
sin_alpha**2 + (sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_alpha1) ** 2
|
|
463
|
-
),
|
|
457
|
+
* np.sqrt(sin_alpha**2 + (sin_U1 * sin_sigma - cos_U1 * cos_sigma * cos_alpha1) ** 2),
|
|
464
458
|
)
|
|
465
459
|
|
|
466
|
-
lam = np.arctan2(
|
|
467
|
-
sin_sigma * sin_alpha1, cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_alpha1
|
|
468
|
-
)
|
|
460
|
+
lam = np.arctan2(sin_sigma * sin_alpha1, cos_U1 * cos_sigma - sin_U1 * sin_sigma * cos_alpha1)
|
|
469
461
|
|
|
470
462
|
C = f / 16 * cos2_alpha * (4 + f * (4 - 3 * cos2_alpha))
|
|
471
463
|
L = lam - (1 - C) * f * sin_alpha * (
|
|
472
|
-
sigma
|
|
473
|
-
+ C * sin_sigma * (cos_2sigma_m + C * cos_sigma * (-1 + 2 * cos_2sigma_m**2))
|
|
464
|
+
sigma + C * sin_sigma * (cos_2sigma_m + C * cos_sigma * (-1 + 2 * cos_2sigma_m**2))
|
|
474
465
|
)
|
|
475
466
|
|
|
476
467
|
lon2 = lon1 + L
|
|
477
468
|
|
|
478
469
|
# Back azimuth
|
|
479
|
-
azimuth2 = np.arctan2(
|
|
480
|
-
sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_alpha1
|
|
481
|
-
)
|
|
470
|
+
azimuth2 = np.arctan2(sin_alpha, -sin_U1 * sin_sigma + cos_U1 * cos_sigma * cos_alpha1)
|
|
482
471
|
|
|
483
472
|
return float(lat2), float(lon2), float(azimuth2)
|
|
484
473
|
|
|
@@ -565,10 +554,7 @@ def inverse_geodetic(
|
|
|
565
554
|
C = f / 16 * cos2_alpha * (4 + f * (4 - 3 * cos2_alpha))
|
|
566
555
|
|
|
567
556
|
lam_new = L + (1 - C) * f * sin_alpha * (
|
|
568
|
-
sigma
|
|
569
|
-
+ C
|
|
570
|
-
* sin_sigma
|
|
571
|
-
* (cos_2sigma_m + C * cos_sigma * (-1 + 2 * cos_2sigma_m**2))
|
|
557
|
+
sigma + C * sin_sigma * (cos_2sigma_m + C * cos_sigma * (-1 + 2 * cos_2sigma_m**2))
|
|
572
558
|
)
|
|
573
559
|
|
|
574
560
|
if abs(lam_new - lam) < 1e-12:
|
|
@@ -588,11 +574,7 @@ def inverse_geodetic(
|
|
|
588
574
|
/ 4
|
|
589
575
|
* (
|
|
590
576
|
cos_sigma * (-1 + 2 * cos_2sigma_m**2)
|
|
591
|
-
- B
|
|
592
|
-
/ 6
|
|
593
|
-
* cos_2sigma_m
|
|
594
|
-
* (-3 + 4 * sin_sigma**2)
|
|
595
|
-
* (-3 + 4 * cos_2sigma_m**2)
|
|
577
|
+
- B / 6 * cos_2sigma_m * (-3 + 4 * sin_sigma**2) * (-3 + 4 * cos_2sigma_m**2)
|
|
596
578
|
)
|
|
597
579
|
)
|
|
598
580
|
)
|
|
@@ -601,9 +583,7 @@ def inverse_geodetic(
|
|
|
601
583
|
|
|
602
584
|
# Azimuths
|
|
603
585
|
azimuth1 = np.arctan2(cos_U2 * sin_lam, cos_U1 * sin_U2 - sin_U1 * cos_U2 * cos_lam)
|
|
604
|
-
azimuth2 = np.arctan2(
|
|
605
|
-
cos_U1 * sin_lam, -sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lam
|
|
606
|
-
)
|
|
586
|
+
azimuth2 = np.arctan2(cos_U1 * sin_lam, -sin_U1 * cos_U2 + cos_U1 * sin_U2 * cos_lam)
|
|
607
587
|
|
|
608
588
|
return float(distance), float(azimuth1), float(azimuth2)
|
|
609
589
|
|
pytcl/navigation/great_circle.py
CHANGED
|
@@ -10,7 +10,9 @@ computing the shortest path on a sphere, including:
|
|
|
10
10
|
- TDOA localization on a sphere
|
|
11
11
|
"""
|
|
12
12
|
|
|
13
|
-
from typing import NamedTuple
|
|
13
|
+
from typing import NamedTuple
|
|
14
|
+
from typing import Optional
|
|
15
|
+
from typing import Tuple
|
|
14
16
|
|
|
15
17
|
import numpy as np
|
|
16
18
|
from numpy.typing import NDArray
|
|
@@ -344,9 +346,7 @@ def great_circle_direct(
|
|
|
344
346
|
"""
|
|
345
347
|
d = distance / radius # Angular distance
|
|
346
348
|
|
|
347
|
-
lat2 = np.arcsin(
|
|
348
|
-
np.sin(lat1) * np.cos(d) + np.cos(lat1) * np.sin(d) * np.cos(azimuth)
|
|
349
|
-
)
|
|
349
|
+
lat2 = np.arcsin(np.sin(lat1) * np.cos(d) + np.cos(lat1) * np.sin(d) * np.cos(azimuth))
|
|
350
350
|
|
|
351
351
|
lon2 = lon1 + np.arctan2(
|
|
352
352
|
np.sin(azimuth) * np.sin(d) * np.cos(lat1),
|
|
@@ -408,9 +408,7 @@ def cross_track_distance(
|
|
|
408
408
|
# Along-track distance
|
|
409
409
|
dat = np.arccos(np.cos(d13) / np.cos(dxt))
|
|
410
410
|
|
|
411
|
-
return CrossTrackResult(
|
|
412
|
-
cross_track=float(dxt * radius), along_track=float(dat * radius)
|
|
413
|
-
)
|
|
411
|
+
return CrossTrackResult(cross_track=float(dxt * radius), along_track=float(dat * radius))
|
|
414
412
|
|
|
415
413
|
|
|
416
414
|
def great_circle_intersect(
|
|
@@ -452,9 +450,7 @@ def great_circle_intersect(
|
|
|
452
450
|
|
|
453
451
|
# Convert to Cartesian unit vectors
|
|
454
452
|
def to_cartesian(lat, lon):
|
|
455
|
-
return np.array(
|
|
456
|
-
[np.cos(lat) * np.cos(lon), np.cos(lat) * np.sin(lon), np.sin(lat)]
|
|
457
|
-
)
|
|
453
|
+
return np.array([np.cos(lat) * np.cos(lon), np.cos(lat) * np.sin(lon), np.sin(lat)])
|
|
458
454
|
|
|
459
455
|
# Normal vectors to the great circles
|
|
460
456
|
p1 = to_cartesian(lat1, lon1)
|
|
@@ -508,9 +504,7 @@ def great_circle_intersect(
|
|
|
508
504
|
lat_i2 = -lat_i1
|
|
509
505
|
lon_i2 = ((lon_i1 + np.pi) % (2 * np.pi)) - np.pi
|
|
510
506
|
|
|
511
|
-
return IntersectionResult(
|
|
512
|
-
float(lat_i1), float(lon_i1), float(lat_i2), float(lon_i2), True
|
|
513
|
-
)
|
|
507
|
+
return IntersectionResult(float(lat_i1), float(lon_i1), float(lat_i2), float(lon_i2), True)
|
|
514
508
|
|
|
515
509
|
|
|
516
510
|
def great_circle_path_intersect(
|
pytcl/navigation/ins.py
CHANGED
|
@@ -18,17 +18,19 @@ References
|
|
|
18
18
|
.. [3] J. Farrell, "Aided Navigation: GPS with High Rate Sensors", McGraw-Hill, 2008.
|
|
19
19
|
"""
|
|
20
20
|
|
|
21
|
-
from typing import NamedTuple
|
|
21
|
+
from typing import NamedTuple
|
|
22
|
+
from typing import Optional
|
|
23
|
+
from typing import Tuple
|
|
22
24
|
|
|
23
25
|
import numpy as np
|
|
24
|
-
from numpy.typing import ArrayLike
|
|
26
|
+
from numpy.typing import ArrayLike
|
|
27
|
+
from numpy.typing import NDArray
|
|
25
28
|
|
|
26
|
-
from pytcl.coordinate_systems.rotations import
|
|
27
|
-
|
|
28
|
-
|
|
29
|
-
|
|
30
|
-
|
|
31
|
-
from pytcl.navigation.geodesy import WGS84, Ellipsoid
|
|
29
|
+
from pytcl.coordinate_systems.rotations import quat2rotmat
|
|
30
|
+
from pytcl.coordinate_systems.rotations import quat_multiply
|
|
31
|
+
from pytcl.coordinate_systems.rotations import rotmat2quat
|
|
32
|
+
from pytcl.navigation.geodesy import WGS84
|
|
33
|
+
from pytcl.navigation.geodesy import Ellipsoid
|
|
32
34
|
|
|
33
35
|
# =============================================================================
|
|
34
36
|
# Physical Constants
|
|
@@ -248,11 +250,7 @@ def normal_gravity(lat: float, alt: float = 0.0) -> float:
|
|
|
248
250
|
|
|
249
251
|
# Free-air correction (first-order)
|
|
250
252
|
g = g0 * (
|
|
251
|
-
1
|
|
252
|
-
- 2
|
|
253
|
-
* alt
|
|
254
|
-
/ A_EARTH
|
|
255
|
-
* (1 + F_EARTH + (OMEGA_EARTH**2 * A_EARTH**2 * B_EARTH) / GM_EARTH)
|
|
253
|
+
1 - 2 * alt / A_EARTH * (1 + F_EARTH + (OMEGA_EARTH**2 * A_EARTH**2 * B_EARTH) / GM_EARTH)
|
|
256
254
|
)
|
|
257
255
|
|
|
258
256
|
return g
|
|
@@ -576,9 +574,7 @@ def update_quaternion(
|
|
|
576
574
|
|
|
577
575
|
if angle < 1e-10:
|
|
578
576
|
# Small angle approximation
|
|
579
|
-
delta_q = np.array(
|
|
580
|
-
[1.0, 0.5 * delta_theta[0], 0.5 * delta_theta[1], 0.5 * delta_theta[2]]
|
|
581
|
-
)
|
|
577
|
+
delta_q = np.array([1.0, 0.5 * delta_theta[0], 0.5 * delta_theta[1], 0.5 * delta_theta[2]])
|
|
582
578
|
else:
|
|
583
579
|
# Exact quaternion for rotation
|
|
584
580
|
axis = delta_theta / angle
|