nrl-tracker 0.22.0__py3-none-any.whl → 0.22.1__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- {nrl_tracker-0.22.0.dist-info → nrl_tracker-0.22.1.dist-info}/METADATA +1 -1
- nrl_tracker-0.22.1.dist-info/RECORD +150 -0
- pytcl/__init__.py +11 -9
- pytcl/assignment_algorithms/__init__.py +42 -32
- pytcl/assignment_algorithms/data_association.py +3 -7
- pytcl/assignment_algorithms/gating.py +2 -4
- pytcl/assignment_algorithms/jpda.py +2 -6
- pytcl/assignment_algorithms/three_dimensional/__init__.py +8 -6
- pytcl/assignment_algorithms/three_dimensional/assignment.py +2 -6
- pytcl/assignment_algorithms/two_dimensional/__init__.py +13 -9
- pytcl/assignment_algorithms/two_dimensional/assignment.py +2 -5
- pytcl/assignment_algorithms/two_dimensional/kbest.py +6 -8
- pytcl/astronomical/__init__.py +108 -96
- pytcl/astronomical/ephemerides.py +1 -3
- pytcl/astronomical/lambert.py +1 -2
- pytcl/astronomical/time_systems.py +1 -2
- pytcl/atmosphere/__init__.py +13 -11
- pytcl/atmosphere/models.py +2 -4
- pytcl/clustering/__init__.py +36 -28
- pytcl/clustering/dbscan.py +2 -5
- pytcl/clustering/gaussian_mixture.py +2 -6
- pytcl/clustering/hierarchical.py +2 -6
- pytcl/clustering/kmeans.py +2 -6
- pytcl/containers/__init__.py +26 -29
- pytcl/containers/cluster_set.py +2 -10
- pytcl/containers/covertree.py +2 -8
- pytcl/containers/kd_tree.py +2 -6
- pytcl/containers/measurement_set.py +2 -9
- pytcl/containers/rtree.py +2 -6
- pytcl/containers/track_list.py +13 -13
- pytcl/containers/vptree.py +2 -7
- pytcl/coordinate_systems/__init__.py +74 -69
- pytcl/coordinate_systems/conversions/__init__.py +24 -20
- pytcl/coordinate_systems/conversions/geodetic.py +2 -4
- pytcl/coordinate_systems/conversions/spherical.py +2 -4
- pytcl/coordinate_systems/jacobians/__init__.py +12 -10
- pytcl/coordinate_systems/jacobians/jacobians.py +1 -2
- pytcl/coordinate_systems/projections/__init__.py +21 -23
- pytcl/coordinate_systems/projections/projections.py +1 -3
- pytcl/coordinate_systems/rotations/__init__.py +22 -20
- pytcl/coordinate_systems/rotations/rotations.py +1 -2
- pytcl/core/__init__.py +22 -16
- pytcl/core/array_utils.py +3 -6
- pytcl/core/validation.py +2 -7
- pytcl/dynamic_estimation/__init__.py +86 -77
- pytcl/dynamic_estimation/imm.py +3 -7
- pytcl/dynamic_estimation/information_filter.py +6 -8
- pytcl/dynamic_estimation/kalman/__init__.py +48 -40
- pytcl/dynamic_estimation/kalman/extended.py +2 -4
- pytcl/dynamic_estimation/kalman/linear.py +3 -7
- pytcl/dynamic_estimation/kalman/square_root.py +2 -5
- pytcl/dynamic_estimation/kalman/unscented.py +3 -8
- pytcl/dynamic_estimation/particle_filters/__init__.py +14 -12
- pytcl/dynamic_estimation/particle_filters/bootstrap.py +2 -6
- pytcl/dynamic_estimation/smoothers.py +3 -8
- pytcl/dynamic_models/__init__.py +41 -37
- pytcl/dynamic_models/continuous_time/__init__.py +11 -11
- pytcl/dynamic_models/continuous_time/dynamics.py +2 -4
- pytcl/dynamic_models/discrete_time/__init__.py +13 -11
- pytcl/dynamic_models/process_noise/__init__.py +13 -11
- pytcl/gravity/__init__.py +65 -55
- pytcl/gravity/clenshaw.py +1 -2
- pytcl/gravity/egm.py +3 -8
- pytcl/gravity/spherical_harmonics.py +1 -2
- pytcl/gravity/tides.py +1 -2
- pytcl/magnetism/__init__.py +25 -26
- pytcl/magnetism/emm.py +1 -4
- pytcl/magnetism/igrf.py +6 -5
- pytcl/magnetism/wmm.py +1 -2
- pytcl/mathematical_functions/__init__.py +87 -69
- pytcl/mathematical_functions/basic_matrix/__init__.py +19 -25
- pytcl/mathematical_functions/basic_matrix/decompositions.py +2 -5
- pytcl/mathematical_functions/basic_matrix/special_matrices.py +1 -2
- pytcl/mathematical_functions/combinatorics/__init__.py +14 -18
- pytcl/mathematical_functions/combinatorics/combinatorics.py +1 -4
- pytcl/mathematical_functions/geometry/__init__.py +15 -15
- pytcl/mathematical_functions/geometry/geometry.py +3 -6
- pytcl/mathematical_functions/interpolation/__init__.py +12 -10
- pytcl/mathematical_functions/interpolation/interpolation.py +2 -7
- pytcl/mathematical_functions/numerical_integration/__init__.py +10 -16
- pytcl/mathematical_functions/numerical_integration/quadrature.py +2 -6
- pytcl/mathematical_functions/signal_processing/__init__.py +30 -42
- pytcl/mathematical_functions/signal_processing/detection.py +3 -6
- pytcl/mathematical_functions/signal_processing/filters.py +2 -5
- pytcl/mathematical_functions/signal_processing/matched_filter.py +3 -6
- pytcl/mathematical_functions/special_functions/__init__.py +76 -74
- pytcl/mathematical_functions/special_functions/bessel.py +1 -2
- pytcl/mathematical_functions/special_functions/debye.py +2 -4
- pytcl/mathematical_functions/special_functions/elliptic.py +1 -2
- pytcl/mathematical_functions/special_functions/error_functions.py +1 -2
- pytcl/mathematical_functions/special_functions/gamma_functions.py +1 -2
- pytcl/mathematical_functions/special_functions/hypergeometric.py +1 -2
- pytcl/mathematical_functions/special_functions/lambert_w.py +1 -2
- pytcl/mathematical_functions/special_functions/marcum_q.py +1 -2
- pytcl/mathematical_functions/statistics/__init__.py +31 -27
- pytcl/mathematical_functions/statistics/distributions.py +4 -9
- pytcl/mathematical_functions/statistics/estimators.py +1 -2
- pytcl/mathematical_functions/transforms/__init__.py +51 -45
- pytcl/mathematical_functions/transforms/fourier.py +2 -5
- pytcl/mathematical_functions/transforms/stft.py +2 -5
- pytcl/mathematical_functions/transforms/wavelets.py +2 -7
- pytcl/navigation/__init__.py +99 -89
- pytcl/navigation/geodesy.py +2 -4
- pytcl/navigation/great_circle.py +1 -3
- pytcl/navigation/ins.py +5 -11
- pytcl/navigation/ins_gnss.py +12 -17
- pytcl/navigation/rhumb.py +2 -4
- pytcl/performance_evaluation/__init__.py +25 -21
- pytcl/performance_evaluation/estimation_metrics.py +1 -3
- pytcl/performance_evaluation/track_metrics.py +1 -3
- pytcl/plotting/__init__.py +38 -30
- pytcl/plotting/coordinates.py +1 -3
- pytcl/plotting/ellipses.py +2 -5
- pytcl/plotting/metrics.py +1 -2
- pytcl/plotting/tracks.py +1 -4
- pytcl/static_estimation/__init__.py +41 -37
- pytcl/static_estimation/least_squares.py +2 -4
- pytcl/static_estimation/maximum_likelihood.py +2 -5
- pytcl/static_estimation/robust.py +2 -5
- pytcl/terrain/__init__.py +34 -28
- pytcl/terrain/dem.py +1 -4
- pytcl/terrain/loaders.py +1 -4
- pytcl/terrain/visibility.py +1 -2
- pytcl/trackers/__init__.py +14 -17
- pytcl/trackers/hypothesis.py +1 -6
- pytcl/trackers/mht.py +9 -13
- pytcl/trackers/multi_target.py +3 -8
- pytcl/trackers/single_target.py +2 -5
- nrl_tracker-0.22.0.dist-info/RECORD +0 -150
- {nrl_tracker-0.22.0.dist-info → nrl_tracker-0.22.1.dist-info}/LICENSE +0 -0
- {nrl_tracker-0.22.0.dist-info → nrl_tracker-0.22.1.dist-info}/WHEEL +0 -0
- {nrl_tracker-0.22.0.dist-info → nrl_tracker-0.22.1.dist-info}/top_level.txt +0 -0
|
@@ -12,97 +12,106 @@ This module provides filtering and smoothing algorithms for state estimation:
|
|
|
12
12
|
"""
|
|
13
13
|
|
|
14
14
|
# Import submodules for easy access
|
|
15
|
-
from pytcl.dynamic_estimation import kalman
|
|
16
|
-
from pytcl.dynamic_estimation import particle_filters
|
|
15
|
+
from pytcl.dynamic_estimation import kalman, particle_filters
|
|
17
16
|
|
|
18
17
|
# IMM estimator
|
|
19
|
-
from pytcl.dynamic_estimation.imm import
|
|
20
|
-
|
|
21
|
-
|
|
22
|
-
|
|
23
|
-
|
|
24
|
-
|
|
25
|
-
|
|
18
|
+
from pytcl.dynamic_estimation.imm import (
|
|
19
|
+
IMMEstimator,
|
|
20
|
+
IMMPrediction,
|
|
21
|
+
IMMState,
|
|
22
|
+
IMMUpdate,
|
|
23
|
+
imm_predict,
|
|
24
|
+
imm_predict_update,
|
|
25
|
+
imm_update,
|
|
26
|
+
)
|
|
26
27
|
|
|
27
28
|
# Information filter
|
|
28
|
-
from pytcl.dynamic_estimation.information_filter import
|
|
29
|
-
|
|
30
|
-
|
|
31
|
-
|
|
32
|
-
|
|
33
|
-
|
|
34
|
-
|
|
35
|
-
|
|
36
|
-
|
|
37
|
-
|
|
38
|
-
|
|
29
|
+
from pytcl.dynamic_estimation.information_filter import (
|
|
30
|
+
InformationFilterResult,
|
|
31
|
+
InformationState,
|
|
32
|
+
SRIFResult,
|
|
33
|
+
SRIFState,
|
|
34
|
+
fuse_information,
|
|
35
|
+
information_filter,
|
|
36
|
+
information_to_state,
|
|
37
|
+
srif_filter,
|
|
38
|
+
srif_predict,
|
|
39
|
+
srif_update,
|
|
40
|
+
state_to_information,
|
|
41
|
+
)
|
|
39
42
|
|
|
40
43
|
# Square-root Kalman filters
|
|
41
44
|
# Cubature Kalman filter
|
|
42
45
|
# Unscented Kalman filter
|
|
43
46
|
# Extended Kalman filter
|
|
44
47
|
# Linear Kalman filter
|
|
45
|
-
from pytcl.dynamic_estimation.kalman import
|
|
46
|
-
|
|
47
|
-
|
|
48
|
-
|
|
49
|
-
|
|
50
|
-
|
|
51
|
-
|
|
52
|
-
|
|
53
|
-
|
|
54
|
-
|
|
55
|
-
|
|
56
|
-
|
|
57
|
-
|
|
58
|
-
|
|
59
|
-
|
|
60
|
-
|
|
61
|
-
|
|
62
|
-
|
|
63
|
-
|
|
64
|
-
|
|
65
|
-
|
|
66
|
-
|
|
67
|
-
|
|
68
|
-
|
|
69
|
-
|
|
70
|
-
|
|
71
|
-
|
|
72
|
-
|
|
73
|
-
|
|
74
|
-
|
|
75
|
-
|
|
76
|
-
|
|
77
|
-
|
|
78
|
-
|
|
79
|
-
|
|
80
|
-
|
|
81
|
-
|
|
48
|
+
from pytcl.dynamic_estimation.kalman import (
|
|
49
|
+
KalmanPrediction,
|
|
50
|
+
KalmanState,
|
|
51
|
+
KalmanUpdate,
|
|
52
|
+
SigmaPoints,
|
|
53
|
+
SRKalmanPrediction,
|
|
54
|
+
SRKalmanState,
|
|
55
|
+
SRKalmanUpdate,
|
|
56
|
+
UDState,
|
|
57
|
+
ckf_predict,
|
|
58
|
+
ckf_spherical_cubature_points,
|
|
59
|
+
ckf_update,
|
|
60
|
+
ekf_predict,
|
|
61
|
+
ekf_predict_auto,
|
|
62
|
+
ekf_update,
|
|
63
|
+
ekf_update_auto,
|
|
64
|
+
information_filter_predict,
|
|
65
|
+
information_filter_update,
|
|
66
|
+
iterated_ekf_update,
|
|
67
|
+
kf_predict,
|
|
68
|
+
kf_predict_update,
|
|
69
|
+
kf_smooth,
|
|
70
|
+
kf_update,
|
|
71
|
+
numerical_jacobian,
|
|
72
|
+
sigma_points_julier,
|
|
73
|
+
sigma_points_merwe,
|
|
74
|
+
sr_ukf_predict,
|
|
75
|
+
sr_ukf_update,
|
|
76
|
+
srkf_predict,
|
|
77
|
+
srkf_predict_update,
|
|
78
|
+
srkf_update,
|
|
79
|
+
ud_factorize,
|
|
80
|
+
ud_predict,
|
|
81
|
+
ud_reconstruct,
|
|
82
|
+
ud_update,
|
|
83
|
+
ukf_predict,
|
|
84
|
+
ukf_update,
|
|
85
|
+
unscented_transform,
|
|
86
|
+
)
|
|
82
87
|
|
|
83
88
|
# Particle filters
|
|
84
|
-
from pytcl.dynamic_estimation.particle_filters import
|
|
85
|
-
|
|
86
|
-
|
|
87
|
-
|
|
88
|
-
|
|
89
|
-
|
|
90
|
-
|
|
91
|
-
|
|
92
|
-
|
|
93
|
-
|
|
94
|
-
|
|
95
|
-
|
|
89
|
+
from pytcl.dynamic_estimation.particle_filters import (
|
|
90
|
+
ParticleState,
|
|
91
|
+
bootstrap_pf_predict,
|
|
92
|
+
bootstrap_pf_step,
|
|
93
|
+
bootstrap_pf_update,
|
|
94
|
+
effective_sample_size,
|
|
95
|
+
gaussian_likelihood,
|
|
96
|
+
initialize_particles,
|
|
97
|
+
particle_covariance,
|
|
98
|
+
particle_mean,
|
|
99
|
+
resample_multinomial,
|
|
100
|
+
resample_residual,
|
|
101
|
+
resample_systematic,
|
|
102
|
+
)
|
|
96
103
|
|
|
97
104
|
# Smoothers
|
|
98
|
-
from pytcl.dynamic_estimation.smoothers import
|
|
99
|
-
|
|
100
|
-
|
|
101
|
-
|
|
102
|
-
|
|
103
|
-
|
|
104
|
-
|
|
105
|
-
|
|
105
|
+
from pytcl.dynamic_estimation.smoothers import (
|
|
106
|
+
FixedLagResult,
|
|
107
|
+
RTSResult,
|
|
108
|
+
SmoothedState,
|
|
109
|
+
fixed_interval_smoother,
|
|
110
|
+
fixed_lag_smoother,
|
|
111
|
+
rts_smoother,
|
|
112
|
+
rts_smoother_single_step,
|
|
113
|
+
two_filter_smoother,
|
|
114
|
+
)
|
|
106
115
|
|
|
107
116
|
# Re-export commonly used functions at the top level
|
|
108
117
|
|
pytcl/dynamic_estimation/imm.py
CHANGED
|
@@ -12,16 +12,12 @@ The IMM algorithm consists of four steps:
|
|
|
12
12
|
4. Output combination
|
|
13
13
|
"""
|
|
14
14
|
|
|
15
|
-
from typing import List
|
|
16
|
-
from typing import NamedTuple
|
|
17
|
-
from typing import Optional
|
|
15
|
+
from typing import List, NamedTuple, Optional
|
|
18
16
|
|
|
19
17
|
import numpy as np
|
|
20
|
-
from numpy.typing import ArrayLike
|
|
21
|
-
from numpy.typing import NDArray
|
|
18
|
+
from numpy.typing import ArrayLike, NDArray
|
|
22
19
|
|
|
23
|
-
from pytcl.dynamic_estimation.kalman.linear import kf_predict
|
|
24
|
-
from pytcl.dynamic_estimation.kalman.linear import kf_update
|
|
20
|
+
from pytcl.dynamic_estimation.kalman.linear import kf_predict, kf_update
|
|
25
21
|
|
|
26
22
|
|
|
27
23
|
class IMMState(NamedTuple):
|
|
@@ -16,17 +16,15 @@ This module provides:
|
|
|
16
16
|
- Square-Root Information Filter (SRIF) for improved numerical stability
|
|
17
17
|
"""
|
|
18
18
|
|
|
19
|
-
from typing import List
|
|
20
|
-
from typing import NamedTuple
|
|
21
|
-
from typing import Optional
|
|
22
|
-
from typing import Tuple
|
|
19
|
+
from typing import List, NamedTuple, Optional, Tuple
|
|
23
20
|
|
|
24
21
|
import numpy as np
|
|
25
|
-
from numpy.typing import ArrayLike
|
|
26
|
-
from numpy.typing import NDArray
|
|
22
|
+
from numpy.typing import ArrayLike, NDArray
|
|
27
23
|
|
|
28
|
-
from pytcl.dynamic_estimation.kalman.linear import
|
|
29
|
-
|
|
24
|
+
from pytcl.dynamic_estimation.kalman.linear import (
|
|
25
|
+
information_filter_predict,
|
|
26
|
+
information_filter_update,
|
|
27
|
+
)
|
|
30
28
|
|
|
31
29
|
|
|
32
30
|
class InformationState(NamedTuple):
|
|
@@ -11,46 +11,54 @@ This module provides:
|
|
|
11
11
|
- U-D factorization filter (Bierman's method)
|
|
12
12
|
"""
|
|
13
13
|
|
|
14
|
-
from pytcl.dynamic_estimation.kalman.extended import
|
|
15
|
-
|
|
16
|
-
|
|
17
|
-
|
|
18
|
-
|
|
19
|
-
|
|
20
|
-
|
|
21
|
-
|
|
22
|
-
from pytcl.dynamic_estimation.kalman.linear import
|
|
23
|
-
|
|
24
|
-
|
|
25
|
-
|
|
26
|
-
|
|
27
|
-
|
|
28
|
-
|
|
29
|
-
|
|
30
|
-
|
|
31
|
-
|
|
32
|
-
|
|
33
|
-
from pytcl.dynamic_estimation.kalman.square_root import
|
|
34
|
-
|
|
35
|
-
|
|
36
|
-
|
|
37
|
-
|
|
38
|
-
|
|
39
|
-
|
|
40
|
-
|
|
41
|
-
|
|
42
|
-
|
|
43
|
-
|
|
44
|
-
|
|
45
|
-
|
|
46
|
-
|
|
47
|
-
|
|
48
|
-
|
|
49
|
-
|
|
50
|
-
|
|
51
|
-
from pytcl.dynamic_estimation.kalman.unscented import
|
|
52
|
-
|
|
53
|
-
|
|
14
|
+
from pytcl.dynamic_estimation.kalman.extended import (
|
|
15
|
+
ekf_predict,
|
|
16
|
+
ekf_predict_auto,
|
|
17
|
+
ekf_update,
|
|
18
|
+
ekf_update_auto,
|
|
19
|
+
iterated_ekf_update,
|
|
20
|
+
numerical_jacobian,
|
|
21
|
+
)
|
|
22
|
+
from pytcl.dynamic_estimation.kalman.linear import (
|
|
23
|
+
KalmanPrediction,
|
|
24
|
+
KalmanState,
|
|
25
|
+
KalmanUpdate,
|
|
26
|
+
information_filter_predict,
|
|
27
|
+
information_filter_update,
|
|
28
|
+
kf_predict,
|
|
29
|
+
kf_predict_update,
|
|
30
|
+
kf_smooth,
|
|
31
|
+
kf_update,
|
|
32
|
+
)
|
|
33
|
+
from pytcl.dynamic_estimation.kalman.square_root import (
|
|
34
|
+
SRKalmanPrediction,
|
|
35
|
+
SRKalmanState,
|
|
36
|
+
SRKalmanUpdate,
|
|
37
|
+
UDState,
|
|
38
|
+
cholesky_update,
|
|
39
|
+
qr_update,
|
|
40
|
+
sr_ukf_predict,
|
|
41
|
+
sr_ukf_update,
|
|
42
|
+
srkf_predict,
|
|
43
|
+
srkf_predict_update,
|
|
44
|
+
srkf_update,
|
|
45
|
+
ud_factorize,
|
|
46
|
+
ud_predict,
|
|
47
|
+
ud_reconstruct,
|
|
48
|
+
ud_update,
|
|
49
|
+
ud_update_scalar,
|
|
50
|
+
)
|
|
51
|
+
from pytcl.dynamic_estimation.kalman.unscented import (
|
|
52
|
+
SigmaPoints,
|
|
53
|
+
ckf_predict,
|
|
54
|
+
ckf_spherical_cubature_points,
|
|
55
|
+
ckf_update,
|
|
56
|
+
sigma_points_julier,
|
|
57
|
+
sigma_points_merwe,
|
|
58
|
+
ukf_predict,
|
|
59
|
+
ukf_update,
|
|
60
|
+
unscented_transform,
|
|
61
|
+
)
|
|
54
62
|
|
|
55
63
|
__all__ = [
|
|
56
64
|
# Linear KF
|
|
@@ -8,11 +8,9 @@ around the current state estimate using Jacobians.
|
|
|
8
8
|
from typing import Callable
|
|
9
9
|
|
|
10
10
|
import numpy as np
|
|
11
|
-
from numpy.typing import ArrayLike
|
|
12
|
-
from numpy.typing import NDArray
|
|
11
|
+
from numpy.typing import ArrayLike, NDArray
|
|
13
12
|
|
|
14
|
-
from pytcl.dynamic_estimation.kalman.linear import KalmanPrediction
|
|
15
|
-
from pytcl.dynamic_estimation.kalman.linear import KalmanUpdate
|
|
13
|
+
from pytcl.dynamic_estimation.kalman.linear import KalmanPrediction, KalmanUpdate
|
|
16
14
|
|
|
17
15
|
|
|
18
16
|
def ekf_predict(
|
|
@@ -5,15 +5,11 @@ This module provides the standard linear Kalman filter for systems with
|
|
|
5
5
|
linear dynamics and linear measurements with Gaussian noise.
|
|
6
6
|
"""
|
|
7
7
|
|
|
8
|
-
from typing import NamedTuple
|
|
9
|
-
from typing import Optional
|
|
10
|
-
from typing import Tuple
|
|
8
|
+
from typing import NamedTuple, Optional, Tuple
|
|
11
9
|
|
|
12
10
|
import numpy as np
|
|
13
|
-
from numpy.typing import ArrayLike
|
|
14
|
-
from
|
|
15
|
-
from scipy.linalg import cho_factor
|
|
16
|
-
from scipy.linalg import cho_solve
|
|
11
|
+
from numpy.typing import ArrayLike, NDArray
|
|
12
|
+
from scipy.linalg import cho_factor, cho_solve
|
|
17
13
|
|
|
18
14
|
|
|
19
15
|
class KalmanState(NamedTuple):
|
|
@@ -12,14 +12,11 @@ Implementations include:
|
|
|
12
12
|
- Square-root versions of UKF and CKF
|
|
13
13
|
"""
|
|
14
14
|
|
|
15
|
-
from typing import Callable
|
|
16
|
-
from typing import NamedTuple
|
|
17
|
-
from typing import Optional
|
|
15
|
+
from typing import Callable, NamedTuple, Optional
|
|
18
16
|
|
|
19
17
|
import numpy as np
|
|
20
18
|
import scipy.linalg
|
|
21
|
-
from numpy.typing import ArrayLike
|
|
22
|
-
from numpy.typing import NDArray
|
|
19
|
+
from numpy.typing import ArrayLike, NDArray
|
|
23
20
|
|
|
24
21
|
|
|
25
22
|
class SRKalmanState(NamedTuple):
|
|
@@ -5,17 +5,12 @@ The UKF uses the unscented transform to propagate the mean and covariance
|
|
|
5
5
|
through nonlinear functions without requiring Jacobian computation.
|
|
6
6
|
"""
|
|
7
7
|
|
|
8
|
-
from typing import Callable
|
|
9
|
-
from typing import NamedTuple
|
|
10
|
-
from typing import Optional
|
|
11
|
-
from typing import Tuple
|
|
8
|
+
from typing import Callable, NamedTuple, Optional, Tuple
|
|
12
9
|
|
|
13
10
|
import numpy as np
|
|
14
|
-
from numpy.typing import ArrayLike
|
|
15
|
-
from numpy.typing import NDArray
|
|
11
|
+
from numpy.typing import ArrayLike, NDArray
|
|
16
12
|
|
|
17
|
-
from pytcl.dynamic_estimation.kalman.linear import KalmanPrediction
|
|
18
|
-
from pytcl.dynamic_estimation.kalman.linear import KalmanUpdate
|
|
13
|
+
from pytcl.dynamic_estimation.kalman.linear import KalmanPrediction, KalmanUpdate
|
|
19
14
|
|
|
20
15
|
|
|
21
16
|
class SigmaPoints(NamedTuple):
|
|
@@ -7,18 +7,20 @@ This module provides:
|
|
|
7
7
|
- Particle statistics (mean, covariance, ESS)
|
|
8
8
|
"""
|
|
9
9
|
|
|
10
|
-
from pytcl.dynamic_estimation.particle_filters.bootstrap import
|
|
11
|
-
|
|
12
|
-
|
|
13
|
-
|
|
14
|
-
|
|
15
|
-
|
|
16
|
-
|
|
17
|
-
|
|
18
|
-
|
|
19
|
-
|
|
20
|
-
|
|
21
|
-
|
|
10
|
+
from pytcl.dynamic_estimation.particle_filters.bootstrap import (
|
|
11
|
+
ParticleState,
|
|
12
|
+
bootstrap_pf_predict,
|
|
13
|
+
bootstrap_pf_step,
|
|
14
|
+
bootstrap_pf_update,
|
|
15
|
+
effective_sample_size,
|
|
16
|
+
gaussian_likelihood,
|
|
17
|
+
initialize_particles,
|
|
18
|
+
particle_covariance,
|
|
19
|
+
particle_mean,
|
|
20
|
+
resample_multinomial,
|
|
21
|
+
resample_residual,
|
|
22
|
+
resample_systematic,
|
|
23
|
+
)
|
|
22
24
|
|
|
23
25
|
__all__ = [
|
|
24
26
|
"ParticleState",
|
|
@@ -5,15 +5,11 @@ This module provides particle filtering algorithms for nonlinear/non-Gaussian
|
|
|
5
5
|
state estimation.
|
|
6
6
|
"""
|
|
7
7
|
|
|
8
|
-
from typing import Callable
|
|
9
|
-
from typing import NamedTuple
|
|
10
|
-
from typing import Optional
|
|
11
|
-
from typing import Tuple
|
|
8
|
+
from typing import Callable, NamedTuple, Optional, Tuple
|
|
12
9
|
|
|
13
10
|
import numpy as np
|
|
14
11
|
from numba import njit
|
|
15
|
-
from numpy.typing import ArrayLike
|
|
16
|
-
from numpy.typing import NDArray
|
|
12
|
+
from numpy.typing import ArrayLike, NDArray
|
|
17
13
|
|
|
18
14
|
|
|
19
15
|
class ParticleState(NamedTuple):
|
|
@@ -12,17 +12,12 @@ The main algorithms are:
|
|
|
12
12
|
- Two-filter smoother for parallel processing
|
|
13
13
|
"""
|
|
14
14
|
|
|
15
|
-
from typing import List
|
|
16
|
-
from typing import NamedTuple
|
|
17
|
-
from typing import Optional
|
|
15
|
+
from typing import List, NamedTuple, Optional
|
|
18
16
|
|
|
19
17
|
import numpy as np
|
|
20
|
-
from numpy.typing import ArrayLike
|
|
21
|
-
from numpy.typing import NDArray
|
|
18
|
+
from numpy.typing import ArrayLike, NDArray
|
|
22
19
|
|
|
23
|
-
from pytcl.dynamic_estimation.kalman.linear import kf_predict
|
|
24
|
-
from pytcl.dynamic_estimation.kalman.linear import kf_smooth
|
|
25
|
-
from pytcl.dynamic_estimation.kalman.linear import kf_update
|
|
20
|
+
from pytcl.dynamic_estimation.kalman.linear import kf_predict, kf_smooth, kf_update
|
|
26
21
|
|
|
27
22
|
|
|
28
23
|
class SmoothedState(NamedTuple):
|
pytcl/dynamic_models/__init__.py
CHANGED
|
@@ -14,49 +14,53 @@ and utilities for discretizing continuous-time models.
|
|
|
14
14
|
"""
|
|
15
15
|
|
|
16
16
|
# Import submodules for easy access
|
|
17
|
-
from pytcl.dynamic_models import continuous_time
|
|
18
|
-
from pytcl.dynamic_models import discrete_time
|
|
19
|
-
from pytcl.dynamic_models import process_noise
|
|
17
|
+
from pytcl.dynamic_models import continuous_time, discrete_time, process_noise
|
|
20
18
|
|
|
21
19
|
# Continuous-time dynamics
|
|
22
|
-
from pytcl.dynamic_models.continuous_time import
|
|
23
|
-
|
|
24
|
-
|
|
25
|
-
|
|
26
|
-
|
|
27
|
-
|
|
28
|
-
|
|
29
|
-
|
|
30
|
-
|
|
31
|
-
|
|
32
|
-
|
|
33
|
-
|
|
20
|
+
from pytcl.dynamic_models.continuous_time import (
|
|
21
|
+
continuous_to_discrete,
|
|
22
|
+
diffusion_constant_acceleration,
|
|
23
|
+
diffusion_constant_velocity,
|
|
24
|
+
diffusion_singer,
|
|
25
|
+
discretize_lti,
|
|
26
|
+
drift_constant_acceleration,
|
|
27
|
+
drift_constant_velocity,
|
|
28
|
+
drift_coordinated_turn_2d,
|
|
29
|
+
drift_singer,
|
|
30
|
+
state_jacobian_ca,
|
|
31
|
+
state_jacobian_cv,
|
|
32
|
+
state_jacobian_singer,
|
|
33
|
+
)
|
|
34
34
|
|
|
35
35
|
# Discrete-time state transition matrices
|
|
36
|
-
from pytcl.dynamic_models.discrete_time import
|
|
37
|
-
|
|
38
|
-
|
|
39
|
-
|
|
40
|
-
|
|
41
|
-
|
|
42
|
-
|
|
43
|
-
|
|
44
|
-
|
|
45
|
-
|
|
46
|
-
|
|
36
|
+
from pytcl.dynamic_models.discrete_time import (
|
|
37
|
+
f_constant_acceleration,
|
|
38
|
+
f_constant_velocity,
|
|
39
|
+
f_coord_turn_2d,
|
|
40
|
+
f_coord_turn_3d,
|
|
41
|
+
f_coord_turn_polar,
|
|
42
|
+
f_discrete_white_noise_accel,
|
|
43
|
+
f_piecewise_white_noise_jerk,
|
|
44
|
+
f_poly_kal,
|
|
45
|
+
f_singer,
|
|
46
|
+
f_singer_2d,
|
|
47
|
+
f_singer_3d,
|
|
48
|
+
)
|
|
47
49
|
|
|
48
50
|
# Process noise covariance matrices
|
|
49
|
-
from pytcl.dynamic_models.process_noise import
|
|
50
|
-
|
|
51
|
-
|
|
52
|
-
|
|
53
|
-
|
|
54
|
-
|
|
55
|
-
|
|
56
|
-
|
|
57
|
-
|
|
58
|
-
|
|
59
|
-
|
|
51
|
+
from pytcl.dynamic_models.process_noise import (
|
|
52
|
+
q_constant_acceleration,
|
|
53
|
+
q_constant_velocity,
|
|
54
|
+
q_continuous_white_noise,
|
|
55
|
+
q_coord_turn_2d,
|
|
56
|
+
q_coord_turn_3d,
|
|
57
|
+
q_coord_turn_polar,
|
|
58
|
+
q_discrete_white_noise,
|
|
59
|
+
q_poly_kal,
|
|
60
|
+
q_singer,
|
|
61
|
+
q_singer_2d,
|
|
62
|
+
q_singer_3d,
|
|
63
|
+
)
|
|
60
64
|
|
|
61
65
|
# Re-export commonly used functions at the top level
|
|
62
66
|
|
|
@@ -5,20 +5,20 @@ This module provides drift and diffusion functions for continuous-time
|
|
|
5
5
|
stochastic differential equations, as well as utilities for discretization.
|
|
6
6
|
"""
|
|
7
7
|
|
|
8
|
-
from pytcl.dynamic_models.continuous_time.dynamics import continuous_to_discrete
|
|
9
8
|
from pytcl.dynamic_models.continuous_time.dynamics import (
|
|
9
|
+
continuous_to_discrete,
|
|
10
10
|
diffusion_constant_acceleration,
|
|
11
|
+
diffusion_constant_velocity,
|
|
12
|
+
diffusion_singer,
|
|
13
|
+
discretize_lti,
|
|
14
|
+
drift_constant_acceleration,
|
|
15
|
+
drift_constant_velocity,
|
|
16
|
+
drift_coordinated_turn_2d,
|
|
17
|
+
drift_singer,
|
|
18
|
+
state_jacobian_ca,
|
|
19
|
+
state_jacobian_cv,
|
|
20
|
+
state_jacobian_singer,
|
|
11
21
|
)
|
|
12
|
-
from pytcl.dynamic_models.continuous_time.dynamics import diffusion_constant_velocity
|
|
13
|
-
from pytcl.dynamic_models.continuous_time.dynamics import diffusion_singer
|
|
14
|
-
from pytcl.dynamic_models.continuous_time.dynamics import discretize_lti
|
|
15
|
-
from pytcl.dynamic_models.continuous_time.dynamics import drift_constant_acceleration
|
|
16
|
-
from pytcl.dynamic_models.continuous_time.dynamics import drift_constant_velocity
|
|
17
|
-
from pytcl.dynamic_models.continuous_time.dynamics import drift_coordinated_turn_2d
|
|
18
|
-
from pytcl.dynamic_models.continuous_time.dynamics import drift_singer
|
|
19
|
-
from pytcl.dynamic_models.continuous_time.dynamics import state_jacobian_ca
|
|
20
|
-
from pytcl.dynamic_models.continuous_time.dynamics import state_jacobian_cv
|
|
21
|
-
from pytcl.dynamic_models.continuous_time.dynamics import state_jacobian_singer
|
|
22
22
|
|
|
23
23
|
__all__ = [
|
|
24
24
|
# Drift functions
|
|
@@ -9,13 +9,11 @@ stochastic differential equations of the form:
|
|
|
9
9
|
where W is a Wiener process.
|
|
10
10
|
"""
|
|
11
11
|
|
|
12
|
-
from typing import Optional
|
|
13
|
-
from typing import Tuple
|
|
12
|
+
from typing import Optional, Tuple
|
|
14
13
|
|
|
15
14
|
import numpy as np
|
|
16
15
|
import scipy.linalg
|
|
17
|
-
from numpy.typing import ArrayLike
|
|
18
|
-
from numpy.typing import NDArray
|
|
16
|
+
from numpy.typing import ArrayLike, NDArray
|
|
19
17
|
|
|
20
18
|
|
|
21
19
|
def drift_constant_velocity(
|
|
@@ -5,17 +5,19 @@ This module provides state transition matrices (F) for various motion models
|
|
|
5
5
|
used in target tracking applications.
|
|
6
6
|
"""
|
|
7
7
|
|
|
8
|
-
from pytcl.dynamic_models.discrete_time.coordinated_turn import
|
|
9
|
-
|
|
10
|
-
|
|
11
|
-
|
|
12
|
-
|
|
13
|
-
from pytcl.dynamic_models.discrete_time.polynomial import
|
|
14
|
-
|
|
15
|
-
|
|
16
|
-
|
|
17
|
-
|
|
18
|
-
|
|
8
|
+
from pytcl.dynamic_models.discrete_time.coordinated_turn import (
|
|
9
|
+
f_coord_turn_2d,
|
|
10
|
+
f_coord_turn_3d,
|
|
11
|
+
f_coord_turn_polar,
|
|
12
|
+
)
|
|
13
|
+
from pytcl.dynamic_models.discrete_time.polynomial import (
|
|
14
|
+
f_constant_acceleration,
|
|
15
|
+
f_constant_velocity,
|
|
16
|
+
f_discrete_white_noise_accel,
|
|
17
|
+
f_piecewise_white_noise_jerk,
|
|
18
|
+
f_poly_kal,
|
|
19
|
+
)
|
|
20
|
+
from pytcl.dynamic_models.discrete_time.singer import f_singer, f_singer_2d, f_singer_3d
|
|
19
21
|
|
|
20
22
|
__all__ = [
|
|
21
23
|
# Polynomial models
|