nrl-tracker 0.21.5__py3-none-any.whl → 0.22.0__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- {nrl_tracker-0.21.5.dist-info → nrl_tracker-0.22.0.dist-info}/METADATA +2 -2
- nrl_tracker-0.22.0.dist-info/RECORD +150 -0
- pytcl/__init__.py +9 -11
- pytcl/assignment_algorithms/__init__.py +32 -42
- pytcl/assignment_algorithms/data_association.py +9 -10
- pytcl/assignment_algorithms/gating.py +7 -5
- pytcl/assignment_algorithms/jpda.py +10 -14
- pytcl/assignment_algorithms/three_dimensional/__init__.py +6 -8
- pytcl/assignment_algorithms/three_dimensional/assignment.py +6 -2
- pytcl/assignment_algorithms/two_dimensional/__init__.py +9 -13
- pytcl/assignment_algorithms/two_dimensional/assignment.py +5 -2
- pytcl/assignment_algorithms/two_dimensional/kbest.py +9 -9
- pytcl/astronomical/__init__.py +130 -89
- pytcl/astronomical/ephemerides.py +524 -0
- pytcl/astronomical/lambert.py +6 -15
- pytcl/astronomical/orbital_mechanics.py +1 -3
- pytcl/astronomical/reference_frames.py +1 -3
- pytcl/astronomical/relativity.py +466 -0
- pytcl/astronomical/time_systems.py +2 -1
- pytcl/atmosphere/__init__.py +12 -14
- pytcl/atmosphere/models.py +5 -5
- pytcl/clustering/__init__.py +28 -36
- pytcl/clustering/dbscan.py +5 -2
- pytcl/clustering/gaussian_mixture.py +10 -10
- pytcl/clustering/hierarchical.py +7 -7
- pytcl/clustering/kmeans.py +7 -5
- pytcl/containers/__init__.py +29 -43
- pytcl/containers/cluster_set.py +13 -20
- pytcl/containers/covertree.py +8 -2
- pytcl/containers/kd_tree.py +6 -2
- pytcl/containers/measurement_set.py +11 -16
- pytcl/containers/rtree.py +8 -7
- pytcl/containers/track_list.py +13 -13
- pytcl/containers/vptree.py +7 -2
- pytcl/coordinate_systems/__init__.py +69 -74
- pytcl/coordinate_systems/conversions/__init__.py +20 -24
- pytcl/coordinate_systems/conversions/geodetic.py +7 -17
- pytcl/coordinate_systems/conversions/spherical.py +4 -2
- pytcl/coordinate_systems/jacobians/__init__.py +10 -12
- pytcl/coordinate_systems/jacobians/jacobians.py +2 -1
- pytcl/coordinate_systems/projections/__init__.py +27 -23
- pytcl/coordinate_systems/projections/projections.py +14 -39
- pytcl/coordinate_systems/rotations/__init__.py +20 -22
- pytcl/coordinate_systems/rotations/rotations.py +3 -4
- pytcl/core/__init__.py +16 -22
- pytcl/core/array_utils.py +7 -7
- pytcl/core/constants.py +1 -3
- pytcl/core/validation.py +13 -19
- pytcl/dynamic_estimation/__init__.py +77 -86
- pytcl/dynamic_estimation/imm.py +10 -15
- pytcl/dynamic_estimation/information_filter.py +8 -6
- pytcl/dynamic_estimation/kalman/__init__.py +40 -48
- pytcl/dynamic_estimation/kalman/extended.py +4 -5
- pytcl/dynamic_estimation/kalman/linear.py +7 -3
- pytcl/dynamic_estimation/kalman/square_root.py +7 -8
- pytcl/dynamic_estimation/kalman/unscented.py +8 -6
- pytcl/dynamic_estimation/particle_filters/__init__.py +12 -14
- pytcl/dynamic_estimation/particle_filters/bootstrap.py +8 -8
- pytcl/dynamic_estimation/smoothers.py +9 -10
- pytcl/dynamic_models/__init__.py +37 -41
- pytcl/dynamic_models/continuous_time/__init__.py +11 -11
- pytcl/dynamic_models/continuous_time/dynamics.py +4 -2
- pytcl/dynamic_models/discrete_time/__init__.py +11 -17
- pytcl/dynamic_models/process_noise/__init__.py +11 -17
- pytcl/dynamic_models/process_noise/polynomial.py +2 -6
- pytcl/gravity/__init__.py +55 -65
- pytcl/gravity/clenshaw.py +4 -7
- pytcl/gravity/egm.py +9 -6
- pytcl/gravity/models.py +1 -3
- pytcl/gravity/spherical_harmonics.py +6 -11
- pytcl/gravity/tides.py +9 -17
- pytcl/magnetism/__init__.py +26 -36
- pytcl/magnetism/emm.py +7 -13
- pytcl/magnetism/igrf.py +5 -6
- pytcl/magnetism/wmm.py +4 -10
- pytcl/mathematical_functions/__init__.py +69 -87
- pytcl/mathematical_functions/basic_matrix/__init__.py +25 -19
- pytcl/mathematical_functions/basic_matrix/decompositions.py +6 -5
- pytcl/mathematical_functions/basic_matrix/special_matrices.py +2 -1
- pytcl/mathematical_functions/combinatorics/__init__.py +18 -14
- pytcl/mathematical_functions/combinatorics/combinatorics.py +5 -4
- pytcl/mathematical_functions/geometry/__init__.py +15 -15
- pytcl/mathematical_functions/geometry/geometry.py +10 -15
- pytcl/mathematical_functions/interpolation/__init__.py +11 -13
- pytcl/mathematical_functions/interpolation/interpolation.py +8 -5
- pytcl/mathematical_functions/numerical_integration/__init__.py +16 -10
- pytcl/mathematical_functions/numerical_integration/quadrature.py +6 -2
- pytcl/mathematical_functions/signal_processing/__init__.py +42 -30
- pytcl/mathematical_functions/signal_processing/detection.py +9 -9
- pytcl/mathematical_functions/signal_processing/filters.py +7 -8
- pytcl/mathematical_functions/signal_processing/matched_filter.py +8 -7
- pytcl/mathematical_functions/special_functions/__init__.py +75 -77
- pytcl/mathematical_functions/special_functions/bessel.py +2 -1
- pytcl/mathematical_functions/special_functions/debye.py +4 -2
- pytcl/mathematical_functions/special_functions/elliptic.py +3 -4
- pytcl/mathematical_functions/special_functions/error_functions.py +2 -1
- pytcl/mathematical_functions/special_functions/gamma_functions.py +3 -4
- pytcl/mathematical_functions/special_functions/hypergeometric.py +2 -1
- pytcl/mathematical_functions/special_functions/lambert_w.py +3 -4
- pytcl/mathematical_functions/special_functions/marcum_q.py +2 -1
- pytcl/mathematical_functions/statistics/__init__.py +27 -31
- pytcl/mathematical_functions/statistics/distributions.py +21 -40
- pytcl/mathematical_functions/statistics/estimators.py +3 -4
- pytcl/mathematical_functions/transforms/__init__.py +45 -51
- pytcl/mathematical_functions/transforms/fourier.py +5 -2
- pytcl/mathematical_functions/transforms/stft.py +8 -11
- pytcl/mathematical_functions/transforms/wavelets.py +13 -20
- pytcl/navigation/__init__.py +96 -102
- pytcl/navigation/geodesy.py +13 -33
- pytcl/navigation/great_circle.py +7 -13
- pytcl/navigation/ins.py +12 -16
- pytcl/navigation/ins_gnss.py +24 -37
- pytcl/navigation/rhumb.py +7 -12
- pytcl/performance_evaluation/__init__.py +21 -25
- pytcl/performance_evaluation/estimation_metrics.py +3 -1
- pytcl/performance_evaluation/track_metrics.py +4 -4
- pytcl/plotting/__init__.py +30 -38
- pytcl/plotting/coordinates.py +8 -18
- pytcl/plotting/ellipses.py +5 -2
- pytcl/plotting/metrics.py +5 -10
- pytcl/plotting/tracks.py +7 -12
- pytcl/static_estimation/__init__.py +37 -41
- pytcl/static_estimation/least_squares.py +5 -4
- pytcl/static_estimation/maximum_likelihood.py +8 -5
- pytcl/static_estimation/robust.py +5 -2
- pytcl/terrain/__init__.py +28 -34
- pytcl/terrain/dem.py +6 -9
- pytcl/terrain/loaders.py +9 -14
- pytcl/terrain/visibility.py +4 -8
- pytcl/trackers/__init__.py +17 -25
- pytcl/trackers/hypothesis.py +8 -8
- pytcl/trackers/mht.py +18 -24
- pytcl/trackers/multi_target.py +8 -6
- pytcl/trackers/single_target.py +5 -2
- nrl_tracker-0.21.5.dist-info/RECORD +0 -148
- {nrl_tracker-0.21.5.dist-info → nrl_tracker-0.22.0.dist-info}/LICENSE +0 -0
- {nrl_tracker-0.21.5.dist-info → nrl_tracker-0.22.0.dist-info}/WHEEL +0 -0
- {nrl_tracker-0.21.5.dist-info → nrl_tracker-0.22.0.dist-info}/top_level.txt +0 -0
pytcl/dynamic_estimation/imm.py
CHANGED
|
@@ -12,15 +12,16 @@ The IMM algorithm consists of four steps:
|
|
|
12
12
|
4. Output combination
|
|
13
13
|
"""
|
|
14
14
|
|
|
15
|
-
from typing import List
|
|
15
|
+
from typing import List
|
|
16
|
+
from typing import NamedTuple
|
|
17
|
+
from typing import Optional
|
|
16
18
|
|
|
17
19
|
import numpy as np
|
|
18
|
-
from numpy.typing import ArrayLike
|
|
20
|
+
from numpy.typing import ArrayLike
|
|
21
|
+
from numpy.typing import NDArray
|
|
19
22
|
|
|
20
|
-
from pytcl.dynamic_estimation.kalman.linear import
|
|
21
|
-
|
|
22
|
-
kf_update,
|
|
23
|
-
)
|
|
23
|
+
from pytcl.dynamic_estimation.kalman.linear import kf_predict
|
|
24
|
+
from pytcl.dynamic_estimation.kalman.linear import kf_update
|
|
24
25
|
|
|
25
26
|
|
|
26
27
|
class IMMState(NamedTuple):
|
|
@@ -472,12 +473,8 @@ def imm_predict_update(
|
|
|
472
473
|
result : IMMUpdate
|
|
473
474
|
Updated states, covariances, and mode probabilities.
|
|
474
475
|
"""
|
|
475
|
-
pred = imm_predict(
|
|
476
|
-
|
|
477
|
-
)
|
|
478
|
-
return imm_update(
|
|
479
|
-
pred.mode_states, pred.mode_covs, pred.mode_probs, z, H_list, R_list
|
|
480
|
-
)
|
|
476
|
+
pred = imm_predict(mode_states, mode_covs, mode_probs, transition_matrix, F_list, Q_list)
|
|
477
|
+
return imm_update(pred.mode_states, pred.mode_covs, pred.mode_probs, z, H_list, R_list)
|
|
481
478
|
|
|
482
479
|
|
|
483
480
|
class IMMEstimator:
|
|
@@ -679,9 +676,7 @@ class IMMEstimator:
|
|
|
679
676
|
Update result.
|
|
680
677
|
"""
|
|
681
678
|
if not self.H_list:
|
|
682
|
-
raise ValueError(
|
|
683
|
-
"Measurement model not set. Call set_measurement_model first."
|
|
684
|
-
)
|
|
679
|
+
raise ValueError("Measurement model not set. Call set_measurement_model first.")
|
|
685
680
|
|
|
686
681
|
result = imm_update(
|
|
687
682
|
self.mode_states,
|
|
@@ -16,15 +16,17 @@ This module provides:
|
|
|
16
16
|
- Square-Root Information Filter (SRIF) for improved numerical stability
|
|
17
17
|
"""
|
|
18
18
|
|
|
19
|
-
from typing import List
|
|
19
|
+
from typing import List
|
|
20
|
+
from typing import NamedTuple
|
|
21
|
+
from typing import Optional
|
|
22
|
+
from typing import Tuple
|
|
20
23
|
|
|
21
24
|
import numpy as np
|
|
22
|
-
from numpy.typing import ArrayLike
|
|
25
|
+
from numpy.typing import ArrayLike
|
|
26
|
+
from numpy.typing import NDArray
|
|
23
27
|
|
|
24
|
-
from pytcl.dynamic_estimation.kalman.linear import
|
|
25
|
-
|
|
26
|
-
information_filter_update,
|
|
27
|
-
)
|
|
28
|
+
from pytcl.dynamic_estimation.kalman.linear import information_filter_predict
|
|
29
|
+
from pytcl.dynamic_estimation.kalman.linear import information_filter_update
|
|
28
30
|
|
|
29
31
|
|
|
30
32
|
class InformationState(NamedTuple):
|
|
@@ -11,54 +11,46 @@ 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
|
-
|
|
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
|
-
)
|
|
14
|
+
from pytcl.dynamic_estimation.kalman.extended import ekf_predict
|
|
15
|
+
from pytcl.dynamic_estimation.kalman.extended import ekf_predict_auto
|
|
16
|
+
from pytcl.dynamic_estimation.kalman.extended import ekf_update
|
|
17
|
+
from pytcl.dynamic_estimation.kalman.extended import ekf_update_auto
|
|
18
|
+
from pytcl.dynamic_estimation.kalman.extended import iterated_ekf_update
|
|
19
|
+
from pytcl.dynamic_estimation.kalman.extended import numerical_jacobian
|
|
20
|
+
from pytcl.dynamic_estimation.kalman.linear import KalmanPrediction
|
|
21
|
+
from pytcl.dynamic_estimation.kalman.linear import KalmanState
|
|
22
|
+
from pytcl.dynamic_estimation.kalman.linear import KalmanUpdate
|
|
23
|
+
from pytcl.dynamic_estimation.kalman.linear import information_filter_predict
|
|
24
|
+
from pytcl.dynamic_estimation.kalman.linear import information_filter_update
|
|
25
|
+
from pytcl.dynamic_estimation.kalman.linear import kf_predict
|
|
26
|
+
from pytcl.dynamic_estimation.kalman.linear import kf_predict_update
|
|
27
|
+
from pytcl.dynamic_estimation.kalman.linear import kf_smooth
|
|
28
|
+
from pytcl.dynamic_estimation.kalman.linear import kf_update
|
|
29
|
+
from pytcl.dynamic_estimation.kalman.square_root import SRKalmanPrediction
|
|
30
|
+
from pytcl.dynamic_estimation.kalman.square_root import SRKalmanState
|
|
31
|
+
from pytcl.dynamic_estimation.kalman.square_root import SRKalmanUpdate
|
|
32
|
+
from pytcl.dynamic_estimation.kalman.square_root import UDState
|
|
33
|
+
from pytcl.dynamic_estimation.kalman.square_root import cholesky_update
|
|
34
|
+
from pytcl.dynamic_estimation.kalman.square_root import qr_update
|
|
35
|
+
from pytcl.dynamic_estimation.kalman.square_root import sr_ukf_predict
|
|
36
|
+
from pytcl.dynamic_estimation.kalman.square_root import sr_ukf_update
|
|
37
|
+
from pytcl.dynamic_estimation.kalman.square_root import srkf_predict
|
|
38
|
+
from pytcl.dynamic_estimation.kalman.square_root import srkf_predict_update
|
|
39
|
+
from pytcl.dynamic_estimation.kalman.square_root import srkf_update
|
|
40
|
+
from pytcl.dynamic_estimation.kalman.square_root import ud_factorize
|
|
41
|
+
from pytcl.dynamic_estimation.kalman.square_root import ud_predict
|
|
42
|
+
from pytcl.dynamic_estimation.kalman.square_root import ud_reconstruct
|
|
43
|
+
from pytcl.dynamic_estimation.kalman.square_root import ud_update
|
|
44
|
+
from pytcl.dynamic_estimation.kalman.square_root import ud_update_scalar
|
|
45
|
+
from pytcl.dynamic_estimation.kalman.unscented import SigmaPoints
|
|
46
|
+
from pytcl.dynamic_estimation.kalman.unscented import ckf_predict
|
|
47
|
+
from pytcl.dynamic_estimation.kalman.unscented import ckf_spherical_cubature_points
|
|
48
|
+
from pytcl.dynamic_estimation.kalman.unscented import ckf_update
|
|
49
|
+
from pytcl.dynamic_estimation.kalman.unscented import sigma_points_julier
|
|
50
|
+
from pytcl.dynamic_estimation.kalman.unscented import sigma_points_merwe
|
|
51
|
+
from pytcl.dynamic_estimation.kalman.unscented import ukf_predict
|
|
52
|
+
from pytcl.dynamic_estimation.kalman.unscented import ukf_update
|
|
53
|
+
from pytcl.dynamic_estimation.kalman.unscented import unscented_transform
|
|
62
54
|
|
|
63
55
|
__all__ = [
|
|
64
56
|
# Linear KF
|
|
@@ -8,12 +8,11 @@ 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
|
|
11
|
+
from numpy.typing import ArrayLike
|
|
12
|
+
from numpy.typing import NDArray
|
|
12
13
|
|
|
13
|
-
from pytcl.dynamic_estimation.kalman.linear import
|
|
14
|
-
|
|
15
|
-
KalmanUpdate,
|
|
16
|
-
)
|
|
14
|
+
from pytcl.dynamic_estimation.kalman.linear import KalmanPrediction
|
|
15
|
+
from pytcl.dynamic_estimation.kalman.linear import KalmanUpdate
|
|
17
16
|
|
|
18
17
|
|
|
19
18
|
def ekf_predict(
|
|
@@ -5,11 +5,15 @@ 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
|
|
8
|
+
from typing import NamedTuple
|
|
9
|
+
from typing import Optional
|
|
10
|
+
from typing import Tuple
|
|
9
11
|
|
|
10
12
|
import numpy as np
|
|
11
|
-
from numpy.typing import ArrayLike
|
|
12
|
-
from
|
|
13
|
+
from numpy.typing import ArrayLike
|
|
14
|
+
from numpy.typing import NDArray
|
|
15
|
+
from scipy.linalg import cho_factor
|
|
16
|
+
from scipy.linalg import cho_solve
|
|
13
17
|
|
|
14
18
|
|
|
15
19
|
class KalmanState(NamedTuple):
|
|
@@ -12,11 +12,14 @@ Implementations include:
|
|
|
12
12
|
- Square-root versions of UKF and CKF
|
|
13
13
|
"""
|
|
14
14
|
|
|
15
|
-
from typing import Callable
|
|
15
|
+
from typing import Callable
|
|
16
|
+
from typing import NamedTuple
|
|
17
|
+
from typing import Optional
|
|
16
18
|
|
|
17
19
|
import numpy as np
|
|
18
20
|
import scipy.linalg
|
|
19
|
-
from numpy.typing import ArrayLike
|
|
21
|
+
from numpy.typing import ArrayLike
|
|
22
|
+
from numpy.typing import NDArray
|
|
20
23
|
|
|
21
24
|
|
|
22
25
|
class SRKalmanState(NamedTuple):
|
|
@@ -705,9 +708,7 @@ def ud_update(
|
|
|
705
708
|
D_upd = D.copy()
|
|
706
709
|
|
|
707
710
|
for i in range(m):
|
|
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
|
-
)
|
|
711
|
+
x_upd, U_upd, D_upd = ud_update_scalar(x_upd, U_upd, D_upd, z[i], H[i, :], R[i, i])
|
|
711
712
|
else:
|
|
712
713
|
# Decorrelate measurements
|
|
713
714
|
S_R = np.linalg.cholesky(R)
|
|
@@ -720,9 +721,7 @@ def ud_update(
|
|
|
720
721
|
D_upd = D.copy()
|
|
721
722
|
|
|
722
723
|
for i in range(m):
|
|
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
|
-
)
|
|
724
|
+
x_upd, U_upd, D_upd = ud_update_scalar(x_upd, U_upd, D_upd, z_dec[i], H_dec[i, :], 1.0)
|
|
726
725
|
|
|
727
726
|
# Compute likelihood
|
|
728
727
|
P = ud_reconstruct(U, D)
|
|
@@ -5,15 +5,17 @@ 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
|
|
8
|
+
from typing import Callable
|
|
9
|
+
from typing import NamedTuple
|
|
10
|
+
from typing import Optional
|
|
11
|
+
from typing import Tuple
|
|
9
12
|
|
|
10
13
|
import numpy as np
|
|
11
|
-
from numpy.typing import ArrayLike
|
|
14
|
+
from numpy.typing import ArrayLike
|
|
15
|
+
from numpy.typing import NDArray
|
|
12
16
|
|
|
13
|
-
from pytcl.dynamic_estimation.kalman.linear import
|
|
14
|
-
|
|
15
|
-
KalmanUpdate,
|
|
16
|
-
)
|
|
17
|
+
from pytcl.dynamic_estimation.kalman.linear import KalmanPrediction
|
|
18
|
+
from pytcl.dynamic_estimation.kalman.linear import KalmanUpdate
|
|
17
19
|
|
|
18
20
|
|
|
19
21
|
class SigmaPoints(NamedTuple):
|
|
@@ -7,20 +7,18 @@ 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
|
-
|
|
22
|
-
resample_systematic,
|
|
23
|
-
)
|
|
10
|
+
from pytcl.dynamic_estimation.particle_filters.bootstrap import ParticleState
|
|
11
|
+
from pytcl.dynamic_estimation.particle_filters.bootstrap import bootstrap_pf_predict
|
|
12
|
+
from pytcl.dynamic_estimation.particle_filters.bootstrap import bootstrap_pf_step
|
|
13
|
+
from pytcl.dynamic_estimation.particle_filters.bootstrap import bootstrap_pf_update
|
|
14
|
+
from pytcl.dynamic_estimation.particle_filters.bootstrap import effective_sample_size
|
|
15
|
+
from pytcl.dynamic_estimation.particle_filters.bootstrap import gaussian_likelihood
|
|
16
|
+
from pytcl.dynamic_estimation.particle_filters.bootstrap import initialize_particles
|
|
17
|
+
from pytcl.dynamic_estimation.particle_filters.bootstrap import particle_covariance
|
|
18
|
+
from pytcl.dynamic_estimation.particle_filters.bootstrap import particle_mean
|
|
19
|
+
from pytcl.dynamic_estimation.particle_filters.bootstrap import resample_multinomial
|
|
20
|
+
from pytcl.dynamic_estimation.particle_filters.bootstrap import resample_residual
|
|
21
|
+
from pytcl.dynamic_estimation.particle_filters.bootstrap import resample_systematic
|
|
24
22
|
|
|
25
23
|
__all__ = [
|
|
26
24
|
"ParticleState",
|
|
@@ -5,11 +5,15 @@ This module provides particle filtering algorithms for nonlinear/non-Gaussian
|
|
|
5
5
|
state estimation.
|
|
6
6
|
"""
|
|
7
7
|
|
|
8
|
-
from typing import Callable
|
|
8
|
+
from typing import Callable
|
|
9
|
+
from typing import NamedTuple
|
|
10
|
+
from typing import Optional
|
|
11
|
+
from typing import Tuple
|
|
9
12
|
|
|
10
13
|
import numpy as np
|
|
11
14
|
from numba import njit
|
|
12
|
-
from numpy.typing import ArrayLike
|
|
15
|
+
from numpy.typing import ArrayLike
|
|
16
|
+
from numpy.typing import NDArray
|
|
13
17
|
|
|
14
18
|
|
|
15
19
|
class ParticleState(NamedTuple):
|
|
@@ -157,9 +161,7 @@ def resample_residual(
|
|
|
157
161
|
residual = Nw - floor_Nw
|
|
158
162
|
|
|
159
163
|
# Deterministic copies (JIT-compiled)
|
|
160
|
-
resampled, idx = _resample_residual_deterministic(
|
|
161
|
-
particles.astype(np.float64), floor_Nw
|
|
162
|
-
)
|
|
164
|
+
resampled, idx = _resample_residual_deterministic(particles.astype(np.float64), floor_Nw)
|
|
163
165
|
|
|
164
166
|
# Multinomial resampling of residuals
|
|
165
167
|
if idx < N:
|
|
@@ -271,9 +273,7 @@ def bootstrap_pf_update(
|
|
|
271
273
|
N = len(particles)
|
|
272
274
|
|
|
273
275
|
# Compute likelihoods
|
|
274
|
-
likelihoods = np.array(
|
|
275
|
-
[likelihood_func(z, particles[i]) for i in range(N)], dtype=np.float64
|
|
276
|
-
)
|
|
276
|
+
likelihoods = np.array([likelihood_func(z, particles[i]) for i in range(N)], dtype=np.float64)
|
|
277
277
|
|
|
278
278
|
# Update weights
|
|
279
279
|
weights_unnorm = weights * likelihoods
|
|
@@ -12,16 +12,17 @@ The main algorithms are:
|
|
|
12
12
|
- Two-filter smoother for parallel processing
|
|
13
13
|
"""
|
|
14
14
|
|
|
15
|
-
from typing import List
|
|
15
|
+
from typing import List
|
|
16
|
+
from typing import NamedTuple
|
|
17
|
+
from typing import Optional
|
|
16
18
|
|
|
17
19
|
import numpy as np
|
|
18
|
-
from numpy.typing import ArrayLike
|
|
20
|
+
from numpy.typing import ArrayLike
|
|
21
|
+
from numpy.typing import NDArray
|
|
19
22
|
|
|
20
|
-
from pytcl.dynamic_estimation.kalman.linear import
|
|
21
|
-
|
|
22
|
-
|
|
23
|
-
kf_update,
|
|
24
|
-
)
|
|
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
|
|
25
26
|
|
|
26
27
|
|
|
27
28
|
class SmoothedState(NamedTuple):
|
|
@@ -650,9 +651,7 @@ def rts_smoother_single_step(
|
|
|
650
651
|
result : SmoothedState
|
|
651
652
|
Smoothed state and covariance at current time.
|
|
652
653
|
"""
|
|
653
|
-
x_s, P_s = kf_smooth(
|
|
654
|
-
x_filt, P_filt, x_pred_next, P_pred_next, x_smooth_next, P_smooth_next, F
|
|
655
|
-
)
|
|
654
|
+
x_s, P_s = kf_smooth(x_filt, P_filt, x_pred_next, P_pred_next, x_smooth_next, P_smooth_next, F)
|
|
656
655
|
return SmoothedState(x=x_s, P=P_s)
|
|
657
656
|
|
|
658
657
|
|
pytcl/dynamic_models/__init__.py
CHANGED
|
@@ -14,53 +14,49 @@ 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
|
|
17
|
+
from pytcl.dynamic_models import continuous_time
|
|
18
|
+
from pytcl.dynamic_models import discrete_time
|
|
19
|
+
from pytcl.dynamic_models import process_noise
|
|
18
20
|
|
|
19
21
|
# Continuous-time dynamics
|
|
20
|
-
from pytcl.dynamic_models.continuous_time import
|
|
21
|
-
|
|
22
|
-
|
|
23
|
-
|
|
24
|
-
|
|
25
|
-
|
|
26
|
-
|
|
27
|
-
|
|
28
|
-
|
|
29
|
-
|
|
30
|
-
|
|
31
|
-
|
|
32
|
-
state_jacobian_singer,
|
|
33
|
-
)
|
|
22
|
+
from pytcl.dynamic_models.continuous_time import continuous_to_discrete
|
|
23
|
+
from pytcl.dynamic_models.continuous_time import diffusion_constant_acceleration
|
|
24
|
+
from pytcl.dynamic_models.continuous_time import diffusion_constant_velocity
|
|
25
|
+
from pytcl.dynamic_models.continuous_time import diffusion_singer
|
|
26
|
+
from pytcl.dynamic_models.continuous_time import discretize_lti
|
|
27
|
+
from pytcl.dynamic_models.continuous_time import drift_constant_acceleration
|
|
28
|
+
from pytcl.dynamic_models.continuous_time import drift_constant_velocity
|
|
29
|
+
from pytcl.dynamic_models.continuous_time import drift_coordinated_turn_2d
|
|
30
|
+
from pytcl.dynamic_models.continuous_time import drift_singer
|
|
31
|
+
from pytcl.dynamic_models.continuous_time import state_jacobian_ca
|
|
32
|
+
from pytcl.dynamic_models.continuous_time import state_jacobian_cv
|
|
33
|
+
from pytcl.dynamic_models.continuous_time import state_jacobian_singer
|
|
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
|
-
|
|
47
|
-
f_singer_3d,
|
|
48
|
-
)
|
|
36
|
+
from pytcl.dynamic_models.discrete_time import f_constant_acceleration
|
|
37
|
+
from pytcl.dynamic_models.discrete_time import f_constant_velocity
|
|
38
|
+
from pytcl.dynamic_models.discrete_time import f_coord_turn_2d
|
|
39
|
+
from pytcl.dynamic_models.discrete_time import f_coord_turn_3d
|
|
40
|
+
from pytcl.dynamic_models.discrete_time import f_coord_turn_polar
|
|
41
|
+
from pytcl.dynamic_models.discrete_time import f_discrete_white_noise_accel
|
|
42
|
+
from pytcl.dynamic_models.discrete_time import f_piecewise_white_noise_jerk
|
|
43
|
+
from pytcl.dynamic_models.discrete_time import f_poly_kal
|
|
44
|
+
from pytcl.dynamic_models.discrete_time import f_singer
|
|
45
|
+
from pytcl.dynamic_models.discrete_time import f_singer_2d
|
|
46
|
+
from pytcl.dynamic_models.discrete_time import f_singer_3d
|
|
49
47
|
|
|
50
48
|
# Process noise covariance matrices
|
|
51
|
-
from pytcl.dynamic_models.process_noise import
|
|
52
|
-
|
|
53
|
-
|
|
54
|
-
|
|
55
|
-
|
|
56
|
-
|
|
57
|
-
|
|
58
|
-
|
|
59
|
-
|
|
60
|
-
|
|
61
|
-
|
|
62
|
-
q_singer_3d,
|
|
63
|
-
)
|
|
49
|
+
from pytcl.dynamic_models.process_noise import q_constant_acceleration
|
|
50
|
+
from pytcl.dynamic_models.process_noise import q_constant_velocity
|
|
51
|
+
from pytcl.dynamic_models.process_noise import q_continuous_white_noise
|
|
52
|
+
from pytcl.dynamic_models.process_noise import q_coord_turn_2d
|
|
53
|
+
from pytcl.dynamic_models.process_noise import q_coord_turn_3d
|
|
54
|
+
from pytcl.dynamic_models.process_noise import q_coord_turn_polar
|
|
55
|
+
from pytcl.dynamic_models.process_noise import q_discrete_white_noise
|
|
56
|
+
from pytcl.dynamic_models.process_noise import q_poly_kal
|
|
57
|
+
from pytcl.dynamic_models.process_noise import q_singer
|
|
58
|
+
from pytcl.dynamic_models.process_noise import q_singer_2d
|
|
59
|
+
from pytcl.dynamic_models.process_noise import q_singer_3d
|
|
64
60
|
|
|
65
61
|
# Re-export commonly used functions at the top level
|
|
66
62
|
|
|
@@ -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
|
|
8
9
|
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,
|
|
21
11
|
)
|
|
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,11 +9,13 @@ stochastic differential equations of the form:
|
|
|
9
9
|
where W is a Wiener process.
|
|
10
10
|
"""
|
|
11
11
|
|
|
12
|
-
from typing import Optional
|
|
12
|
+
from typing import Optional
|
|
13
|
+
from typing import Tuple
|
|
13
14
|
|
|
14
15
|
import numpy as np
|
|
15
16
|
import scipy.linalg
|
|
16
|
-
from numpy.typing import ArrayLike
|
|
17
|
+
from numpy.typing import ArrayLike
|
|
18
|
+
from numpy.typing import NDArray
|
|
17
19
|
|
|
18
20
|
|
|
19
21
|
def drift_constant_velocity(
|
|
@@ -5,23 +5,17 @@ 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
|
-
|
|
19
|
-
)
|
|
20
|
-
from pytcl.dynamic_models.discrete_time.singer import (
|
|
21
|
-
f_singer,
|
|
22
|
-
f_singer_2d,
|
|
23
|
-
f_singer_3d,
|
|
24
|
-
)
|
|
8
|
+
from pytcl.dynamic_models.discrete_time.coordinated_turn import f_coord_turn_2d
|
|
9
|
+
from pytcl.dynamic_models.discrete_time.coordinated_turn import f_coord_turn_3d
|
|
10
|
+
from pytcl.dynamic_models.discrete_time.coordinated_turn import f_coord_turn_polar
|
|
11
|
+
from pytcl.dynamic_models.discrete_time.polynomial import f_constant_acceleration
|
|
12
|
+
from pytcl.dynamic_models.discrete_time.polynomial import f_constant_velocity
|
|
13
|
+
from pytcl.dynamic_models.discrete_time.polynomial import f_discrete_white_noise_accel
|
|
14
|
+
from pytcl.dynamic_models.discrete_time.polynomial import f_piecewise_white_noise_jerk
|
|
15
|
+
from pytcl.dynamic_models.discrete_time.polynomial import f_poly_kal
|
|
16
|
+
from pytcl.dynamic_models.discrete_time.singer import f_singer
|
|
17
|
+
from pytcl.dynamic_models.discrete_time.singer import f_singer_2d
|
|
18
|
+
from pytcl.dynamic_models.discrete_time.singer import f_singer_3d
|
|
25
19
|
|
|
26
20
|
__all__ = [
|
|
27
21
|
# Polynomial models
|
|
@@ -5,23 +5,17 @@ This module provides process noise covariance matrices (Q) for various
|
|
|
5
5
|
motion models used in target tracking applications.
|
|
6
6
|
"""
|
|
7
7
|
|
|
8
|
-
from pytcl.dynamic_models.process_noise.coordinated_turn import
|
|
9
|
-
|
|
10
|
-
|
|
11
|
-
|
|
12
|
-
|
|
13
|
-
from pytcl.dynamic_models.process_noise.polynomial import
|
|
14
|
-
|
|
15
|
-
|
|
16
|
-
|
|
17
|
-
|
|
18
|
-
|
|
19
|
-
)
|
|
20
|
-
from pytcl.dynamic_models.process_noise.singer import (
|
|
21
|
-
q_singer,
|
|
22
|
-
q_singer_2d,
|
|
23
|
-
q_singer_3d,
|
|
24
|
-
)
|
|
8
|
+
from pytcl.dynamic_models.process_noise.coordinated_turn import q_coord_turn_2d
|
|
9
|
+
from pytcl.dynamic_models.process_noise.coordinated_turn import q_coord_turn_3d
|
|
10
|
+
from pytcl.dynamic_models.process_noise.coordinated_turn import q_coord_turn_polar
|
|
11
|
+
from pytcl.dynamic_models.process_noise.polynomial import q_constant_acceleration
|
|
12
|
+
from pytcl.dynamic_models.process_noise.polynomial import q_constant_velocity
|
|
13
|
+
from pytcl.dynamic_models.process_noise.polynomial import q_continuous_white_noise
|
|
14
|
+
from pytcl.dynamic_models.process_noise.polynomial import q_discrete_white_noise
|
|
15
|
+
from pytcl.dynamic_models.process_noise.polynomial import q_poly_kal
|
|
16
|
+
from pytcl.dynamic_models.process_noise.singer import q_singer
|
|
17
|
+
from pytcl.dynamic_models.process_noise.singer import q_singer_2d
|
|
18
|
+
from pytcl.dynamic_models.process_noise.singer import q_singer_3d
|
|
25
19
|
|
|
26
20
|
__all__ = [
|
|
27
21
|
# Polynomial models
|
|
@@ -71,9 +71,7 @@ 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] = (
|
|
75
|
-
q * T**power / (power * math.factorial(pi) * math.factorial(pj))
|
|
76
|
-
)
|
|
74
|
+
Q_1d[i, j] = q * T**power / (power * math.factorial(pi) * math.factorial(pj))
|
|
77
75
|
|
|
78
76
|
if num_dims == 1:
|
|
79
77
|
return Q_1d
|
|
@@ -276,9 +274,7 @@ def q_continuous_white_noise(
|
|
|
276
274
|
"""
|
|
277
275
|
# This is the same as q_discrete_white_noise but with spectral density
|
|
278
276
|
# instead of variance (multiply by T for conversion in simple cases)
|
|
279
|
-
return q_discrete_white_noise(
|
|
280
|
-
dim=dim, T=T, var=spectral_density, block_size=block_size
|
|
281
|
-
)
|
|
277
|
+
return q_discrete_white_noise(dim=dim, T=T, var=spectral_density, block_size=block_size)
|
|
282
278
|
|
|
283
279
|
|
|
284
280
|
__all__ = [
|