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/navigation/ins_gnss.py
CHANGED
|
@@ -17,20 +17,25 @@ References
|
|
|
17
17
|
Kalman Filtering", 4th ed., Wiley, 2012.
|
|
18
18
|
"""
|
|
19
19
|
|
|
20
|
-
from typing import List
|
|
20
|
+
from typing import List
|
|
21
|
+
from typing import NamedTuple
|
|
22
|
+
from typing import Optional
|
|
23
|
+
from typing import Tuple
|
|
21
24
|
|
|
22
25
|
import numpy as np
|
|
23
|
-
from numpy.typing import ArrayLike
|
|
24
|
-
|
|
25
|
-
|
|
26
|
-
from pytcl.
|
|
27
|
-
from pytcl.
|
|
28
|
-
|
|
29
|
-
|
|
30
|
-
|
|
31
|
-
|
|
32
|
-
|
|
33
|
-
|
|
26
|
+
from numpy.typing import ArrayLike
|
|
27
|
+
from numpy.typing import NDArray
|
|
28
|
+
|
|
29
|
+
from pytcl.dynamic_estimation.kalman import kf_predict
|
|
30
|
+
from pytcl.dynamic_estimation.kalman import kf_update
|
|
31
|
+
from pytcl.navigation.geodesy import WGS84
|
|
32
|
+
from pytcl.navigation.geodesy import Ellipsoid
|
|
33
|
+
from pytcl.navigation.geodesy import geodetic_to_ecef
|
|
34
|
+
from pytcl.navigation.ins import IMUData
|
|
35
|
+
from pytcl.navigation.ins import INSState
|
|
36
|
+
from pytcl.navigation.ins import ins_error_state_matrix
|
|
37
|
+
from pytcl.navigation.ins import ins_process_noise_matrix
|
|
38
|
+
from pytcl.navigation.ins import mechanize_ins_ned
|
|
34
39
|
|
|
35
40
|
# =============================================================================
|
|
36
41
|
# Constants
|
|
@@ -512,9 +517,7 @@ def loose_coupled_predict(
|
|
|
512
517
|
dt = imu.dt
|
|
513
518
|
|
|
514
519
|
# Propagate INS mechanization
|
|
515
|
-
ins_new = mechanize_ins_ned(
|
|
516
|
-
state.ins_state, imu, accel_prev=accel_prev, gyro_prev=gyro_prev
|
|
517
|
-
)
|
|
520
|
+
ins_new = mechanize_ins_ned(state.ins_state, imu, accel_prev=accel_prev, gyro_prev=gyro_prev)
|
|
518
521
|
|
|
519
522
|
# Get error state transition matrix (continuous-time)
|
|
520
523
|
F_cont = ins_error_state_matrix(state.ins_state)
|
|
@@ -576,9 +579,7 @@ def loose_coupled_update_position(
|
|
|
576
579
|
if gnss.position_cov is not None:
|
|
577
580
|
R = gnss.position_cov
|
|
578
581
|
else:
|
|
579
|
-
R = np.diag(
|
|
580
|
-
[10.0**2, 10.0**2, 15.0**2]
|
|
581
|
-
) # Default: 10m horizontal, 15m vertical
|
|
582
|
+
R = np.diag([10.0**2, 10.0**2, 15.0**2]) # Default: 10m horizontal, 15m vertical
|
|
582
583
|
|
|
583
584
|
# Innovation: measured position - INS predicted position
|
|
584
585
|
z = gnss.position - state.ins_state.position
|
|
@@ -695,16 +696,8 @@ def loose_coupled_update(
|
|
|
695
696
|
# Full position + velocity update
|
|
696
697
|
H = position_velocity_measurement_matrix()
|
|
697
698
|
|
|
698
|
-
R_pos = (
|
|
699
|
-
|
|
700
|
-
if gnss.position_cov is not None
|
|
701
|
-
else np.diag([10.0**2] * 3)
|
|
702
|
-
)
|
|
703
|
-
R_vel = (
|
|
704
|
-
gnss.velocity_cov
|
|
705
|
-
if gnss.velocity_cov is not None
|
|
706
|
-
else np.diag([0.1**2] * 3)
|
|
707
|
-
)
|
|
699
|
+
R_pos = gnss.position_cov if gnss.position_cov is not None else np.diag([10.0**2] * 3)
|
|
700
|
+
R_vel = gnss.velocity_cov if gnss.velocity_cov is not None else np.diag([0.1**2] * 3)
|
|
708
701
|
R = np.block([[R_pos, np.zeros((3, 3))], [np.zeros((3, 3)), R_vel]])
|
|
709
702
|
|
|
710
703
|
z = np.concatenate(
|
|
@@ -846,9 +839,7 @@ def tight_coupled_measurement_matrix(
|
|
|
846
839
|
los, _ = compute_line_of_sight(user_ecef, sat.position)
|
|
847
840
|
|
|
848
841
|
# LOS components in ECEF
|
|
849
|
-
los_x, los_y, los_z =
|
|
850
|
-
-los
|
|
851
|
-
) # Negative because increase in user pos decreases range
|
|
842
|
+
los_x, los_y, los_z = -los # Negative because increase in user pos decreases range
|
|
852
843
|
|
|
853
844
|
# Transform LOS to geodetic derivatives
|
|
854
845
|
# d(range)/d(lat), d(range)/d(lon), d(range)/d(alt)
|
|
@@ -858,9 +849,7 @@ def tight_coupled_measurement_matrix(
|
|
|
858
849
|
+ los_z * (cos_lat * N * (1 - ellipsoid.e2))
|
|
859
850
|
)
|
|
860
851
|
H[i, 1] = los_x * (-cos_lat * sin_lon * N) + los_y * (cos_lat * cos_lon * N)
|
|
861
|
-
H[i, 2] =
|
|
862
|
-
los_x * cos_lat * cos_lon + los_y * cos_lat * sin_lon + los_z * sin_lat
|
|
863
|
-
)
|
|
852
|
+
H[i, 2] = los_x * cos_lat * cos_lon + los_y * cos_lat * sin_lon + los_z * sin_lat
|
|
864
853
|
|
|
865
854
|
# Clock bias (state 15)
|
|
866
855
|
H[i, 15] = 1.0
|
|
@@ -991,9 +980,7 @@ def _apply_error_correction(
|
|
|
991
980
|
|
|
992
981
|
# Apply small angle rotation to quaternion
|
|
993
982
|
q = ins_state.quaternion
|
|
994
|
-
delta_q = np.array(
|
|
995
|
-
[1.0, 0.5 * phi[0], 0.5 * phi[1], 0.5 * phi[2]], dtype=np.float64
|
|
996
|
-
)
|
|
983
|
+
delta_q = np.array([1.0, 0.5 * phi[0], 0.5 * phi[1], 0.5 * phi[2]], dtype=np.float64)
|
|
997
984
|
delta_q = delta_q / np.linalg.norm(delta_q)
|
|
998
985
|
|
|
999
986
|
# Quaternion multiplication (body frame correction)
|
pytcl/navigation/rhumb.py
CHANGED
|
@@ -9,12 +9,14 @@ constant-bearing paths on a sphere and ellipsoid, including:
|
|
|
9
9
|
- Spherical and ellipsoidal formulations
|
|
10
10
|
"""
|
|
11
11
|
|
|
12
|
-
from typing import NamedTuple
|
|
12
|
+
from typing import NamedTuple
|
|
13
|
+
from typing import Tuple
|
|
13
14
|
|
|
14
15
|
import numpy as np
|
|
15
16
|
from numpy.typing import NDArray
|
|
16
17
|
|
|
17
|
-
from pytcl.navigation.geodesy import WGS84
|
|
18
|
+
from pytcl.navigation.geodesy import WGS84
|
|
19
|
+
from pytcl.navigation.geodesy import Ellipsoid
|
|
18
20
|
|
|
19
21
|
|
|
20
22
|
class RhumbResult(NamedTuple):
|
|
@@ -100,9 +102,7 @@ def _isometric_latitude(lat: float, e2: float = 0.0) -> float:
|
|
|
100
102
|
)
|
|
101
103
|
|
|
102
104
|
|
|
103
|
-
def _inverse_isometric_latitude(
|
|
104
|
-
psi: float, e2: float = 0.0, max_iter: int = 20
|
|
105
|
-
) -> float:
|
|
105
|
+
def _inverse_isometric_latitude(psi: float, e2: float = 0.0, max_iter: int = 20) -> float:
|
|
106
106
|
"""
|
|
107
107
|
Compute geodetic latitude from isometric latitude.
|
|
108
108
|
|
|
@@ -131,10 +131,7 @@ def _inverse_isometric_latitude(
|
|
|
131
131
|
for _ in range(max_iter):
|
|
132
132
|
sin_lat = np.sin(lat)
|
|
133
133
|
lat_new = (
|
|
134
|
-
2
|
|
135
|
-
* np.arctan(
|
|
136
|
-
((1 + e * sin_lat) / (1 - e * sin_lat)) ** (e / 2) * np.exp(psi)
|
|
137
|
-
)
|
|
134
|
+
2 * np.arctan(((1 + e * sin_lat) / (1 - e * sin_lat)) ** (e / 2) * np.exp(psi))
|
|
138
135
|
- np.pi / 2
|
|
139
136
|
)
|
|
140
137
|
if abs(lat_new - lat) < 1e-12:
|
|
@@ -427,9 +424,7 @@ def indirect_rhumb(
|
|
|
427
424
|
if abs(dlon) > np.pi:
|
|
428
425
|
dlon = dlon - np.sign(dlon) * 2 * np.pi
|
|
429
426
|
|
|
430
|
-
dpsi = _isometric_latitude(lat2, ellipsoid.e2) - _isometric_latitude(
|
|
431
|
-
lat1, ellipsoid.e2
|
|
432
|
-
)
|
|
427
|
+
dpsi = _isometric_latitude(lat2, ellipsoid.e2) - _isometric_latitude(lat1, ellipsoid.e2)
|
|
433
428
|
bearing = np.arctan2(dlon, dpsi) % (2 * np.pi)
|
|
434
429
|
|
|
435
430
|
return RhumbResult(distance, bearing)
|
|
@@ -27,33 +27,29 @@ RMSE: 0.100
|
|
|
27
27
|
"""
|
|
28
28
|
|
|
29
29
|
# Estimation metrics
|
|
30
|
-
from pytcl.performance_evaluation.estimation_metrics import
|
|
31
|
-
|
|
32
|
-
|
|
33
|
-
|
|
34
|
-
|
|
35
|
-
|
|
36
|
-
|
|
37
|
-
|
|
38
|
-
|
|
39
|
-
|
|
40
|
-
|
|
41
|
-
|
|
42
|
-
|
|
43
|
-
velocity_rmse,
|
|
44
|
-
)
|
|
30
|
+
from pytcl.performance_evaluation.estimation_metrics import ConsistencyResult
|
|
31
|
+
from pytcl.performance_evaluation.estimation_metrics import average_nees
|
|
32
|
+
from pytcl.performance_evaluation.estimation_metrics import consistency_test
|
|
33
|
+
from pytcl.performance_evaluation.estimation_metrics import credibility_interval
|
|
34
|
+
from pytcl.performance_evaluation.estimation_metrics import estimation_error_bounds
|
|
35
|
+
from pytcl.performance_evaluation.estimation_metrics import monte_carlo_rmse
|
|
36
|
+
from pytcl.performance_evaluation.estimation_metrics import nees
|
|
37
|
+
from pytcl.performance_evaluation.estimation_metrics import nees_sequence
|
|
38
|
+
from pytcl.performance_evaluation.estimation_metrics import nis
|
|
39
|
+
from pytcl.performance_evaluation.estimation_metrics import nis_sequence
|
|
40
|
+
from pytcl.performance_evaluation.estimation_metrics import position_rmse
|
|
41
|
+
from pytcl.performance_evaluation.estimation_metrics import rmse
|
|
42
|
+
from pytcl.performance_evaluation.estimation_metrics import velocity_rmse
|
|
45
43
|
|
|
46
44
|
# Track metrics
|
|
47
|
-
from pytcl.performance_evaluation.track_metrics import
|
|
48
|
-
|
|
49
|
-
|
|
50
|
-
|
|
51
|
-
|
|
52
|
-
|
|
53
|
-
|
|
54
|
-
|
|
55
|
-
track_purity,
|
|
56
|
-
)
|
|
45
|
+
from pytcl.performance_evaluation.track_metrics import MOTMetrics
|
|
46
|
+
from pytcl.performance_evaluation.track_metrics import OSPAResult
|
|
47
|
+
from pytcl.performance_evaluation.track_metrics import identity_switches
|
|
48
|
+
from pytcl.performance_evaluation.track_metrics import mot_metrics
|
|
49
|
+
from pytcl.performance_evaluation.track_metrics import ospa
|
|
50
|
+
from pytcl.performance_evaluation.track_metrics import ospa_over_time
|
|
51
|
+
from pytcl.performance_evaluation.track_metrics import track_fragmentation
|
|
52
|
+
from pytcl.performance_evaluation.track_metrics import track_purity
|
|
57
53
|
|
|
58
54
|
__all__ = [
|
|
59
55
|
# Track metrics
|
|
@@ -10,7 +10,9 @@ References
|
|
|
10
10
|
Applications to Tracking and Navigation," Wiley, 2001.
|
|
11
11
|
"""
|
|
12
12
|
|
|
13
|
-
from typing import List
|
|
13
|
+
from typing import List
|
|
14
|
+
from typing import NamedTuple
|
|
15
|
+
from typing import Optional
|
|
14
16
|
|
|
15
17
|
import numpy as np
|
|
16
18
|
from numpy.typing import NDArray
|
|
@@ -14,7 +14,9 @@ References
|
|
|
14
14
|
2008.
|
|
15
15
|
"""
|
|
16
16
|
|
|
17
|
-
from typing import List
|
|
17
|
+
from typing import List
|
|
18
|
+
from typing import NamedTuple
|
|
19
|
+
from typing import Optional
|
|
18
20
|
|
|
19
21
|
import numpy as np
|
|
20
22
|
from numpy.typing import NDArray
|
|
@@ -158,9 +160,7 @@ def ospa(
|
|
|
158
160
|
loc_component = (localization_sum / n) ** (1.0 / p) if localization_sum > 0 else 0.0
|
|
159
161
|
card_component = (cardinality_penalty / n) ** (1.0 / p) if n > m else 0.0
|
|
160
162
|
|
|
161
|
-
return OSPAResult(
|
|
162
|
-
ospa=ospa_val, localization=loc_component, cardinality=card_component
|
|
163
|
-
)
|
|
163
|
+
return OSPAResult(ospa=ospa_val, localization=loc_component, cardinality=card_component)
|
|
164
164
|
|
|
165
165
|
|
|
166
166
|
def ospa_over_time(
|
pytcl/plotting/__init__.py
CHANGED
|
@@ -32,50 +32,42 @@ Plotly is required for all plotting functions. Install with:
|
|
|
32
32
|
"""
|
|
33
33
|
|
|
34
34
|
# Coordinate system visualization
|
|
35
|
-
from pytcl.plotting.coordinates import
|
|
36
|
-
|
|
37
|
-
|
|
38
|
-
|
|
39
|
-
|
|
40
|
-
|
|
41
|
-
|
|
42
|
-
plot_spherical_grid,
|
|
43
|
-
)
|
|
35
|
+
from pytcl.plotting.coordinates import plot_coordinate_axes_3d
|
|
36
|
+
from pytcl.plotting.coordinates import plot_coordinate_transform
|
|
37
|
+
from pytcl.plotting.coordinates import plot_euler_angles
|
|
38
|
+
from pytcl.plotting.coordinates import plot_points_spherical
|
|
39
|
+
from pytcl.plotting.coordinates import plot_quaternion_interpolation
|
|
40
|
+
from pytcl.plotting.coordinates import plot_rotation_comparison
|
|
41
|
+
from pytcl.plotting.coordinates import plot_spherical_grid
|
|
44
42
|
|
|
45
43
|
# Covariance ellipse utilities
|
|
46
|
-
from pytcl.plotting.ellipses import
|
|
47
|
-
|
|
48
|
-
|
|
49
|
-
|
|
50
|
-
|
|
51
|
-
|
|
52
|
-
|
|
53
|
-
plot_covariance_ellipsoid,
|
|
54
|
-
)
|
|
44
|
+
from pytcl.plotting.ellipses import confidence_region_radius
|
|
45
|
+
from pytcl.plotting.ellipses import covariance_ellipse_points
|
|
46
|
+
from pytcl.plotting.ellipses import covariance_ellipsoid_points
|
|
47
|
+
from pytcl.plotting.ellipses import ellipse_parameters
|
|
48
|
+
from pytcl.plotting.ellipses import plot_covariance_ellipse
|
|
49
|
+
from pytcl.plotting.ellipses import plot_covariance_ellipses
|
|
50
|
+
from pytcl.plotting.ellipses import plot_covariance_ellipsoid
|
|
55
51
|
|
|
56
52
|
# Performance metric visualization
|
|
57
|
-
from pytcl.plotting.metrics import
|
|
58
|
-
|
|
59
|
-
|
|
60
|
-
|
|
61
|
-
|
|
62
|
-
|
|
63
|
-
|
|
64
|
-
|
|
65
|
-
plot_rmse_over_time,
|
|
66
|
-
)
|
|
53
|
+
from pytcl.plotting.metrics import plot_cardinality_over_time
|
|
54
|
+
from pytcl.plotting.metrics import plot_consistency_summary
|
|
55
|
+
from pytcl.plotting.metrics import plot_error_histogram
|
|
56
|
+
from pytcl.plotting.metrics import plot_monte_carlo_rmse
|
|
57
|
+
from pytcl.plotting.metrics import plot_nees_sequence
|
|
58
|
+
from pytcl.plotting.metrics import plot_nis_sequence
|
|
59
|
+
from pytcl.plotting.metrics import plot_ospa_over_time
|
|
60
|
+
from pytcl.plotting.metrics import plot_rmse_over_time
|
|
67
61
|
|
|
68
62
|
# Track and trajectory plotting
|
|
69
|
-
from pytcl.plotting.tracks import
|
|
70
|
-
|
|
71
|
-
|
|
72
|
-
|
|
73
|
-
|
|
74
|
-
|
|
75
|
-
|
|
76
|
-
|
|
77
|
-
plot_trajectory_3d,
|
|
78
|
-
)
|
|
63
|
+
from pytcl.plotting.tracks import create_animated_tracking
|
|
64
|
+
from pytcl.plotting.tracks import plot_estimation_comparison
|
|
65
|
+
from pytcl.plotting.tracks import plot_measurements_2d
|
|
66
|
+
from pytcl.plotting.tracks import plot_multi_target_tracks
|
|
67
|
+
from pytcl.plotting.tracks import plot_state_time_series
|
|
68
|
+
from pytcl.plotting.tracks import plot_tracking_result
|
|
69
|
+
from pytcl.plotting.tracks import plot_trajectory_2d
|
|
70
|
+
from pytcl.plotting.tracks import plot_trajectory_3d
|
|
79
71
|
|
|
80
72
|
__all__ = [
|
|
81
73
|
# Ellipses
|
pytcl/plotting/coordinates.py
CHANGED
|
@@ -5,7 +5,9 @@ This module provides functions for visualizing coordinate systems,
|
|
|
5
5
|
rotations, and transformations in 2D and 3D.
|
|
6
6
|
"""
|
|
7
7
|
|
|
8
|
-
from typing import List
|
|
8
|
+
from typing import List
|
|
9
|
+
from typing import Optional
|
|
10
|
+
from typing import Tuple
|
|
9
11
|
|
|
10
12
|
import numpy as np
|
|
11
13
|
from numpy.typing import ArrayLike
|
|
@@ -181,19 +183,13 @@ def plot_euler_angles(
|
|
|
181
183
|
|
|
182
184
|
# Create rotation matrices for each axis
|
|
183
185
|
def rotx(a):
|
|
184
|
-
return np.array(
|
|
185
|
-
[[1, 0, 0], [0, np.cos(a), -np.sin(a)], [0, np.sin(a), np.cos(a)]]
|
|
186
|
-
)
|
|
186
|
+
return np.array([[1, 0, 0], [0, np.cos(a), -np.sin(a)], [0, np.sin(a), np.cos(a)]])
|
|
187
187
|
|
|
188
188
|
def roty(a):
|
|
189
|
-
return np.array(
|
|
190
|
-
[[np.cos(a), 0, np.sin(a)], [0, 1, 0], [-np.sin(a), 0, np.cos(a)]]
|
|
191
|
-
)
|
|
189
|
+
return np.array([[np.cos(a), 0, np.sin(a)], [0, 1, 0], [-np.sin(a), 0, np.cos(a)]])
|
|
192
190
|
|
|
193
191
|
def rotz(a):
|
|
194
|
-
return np.array(
|
|
195
|
-
[[np.cos(a), -np.sin(a), 0], [np.sin(a), np.cos(a), 0], [0, 0, 1]]
|
|
196
|
-
)
|
|
192
|
+
return np.array([[np.cos(a), -np.sin(a), 0], [np.sin(a), np.cos(a), 0], [0, 0, 1]])
|
|
197
193
|
|
|
198
194
|
rot_funcs = {"X": rotx, "Y": roty, "Z": rotz}
|
|
199
195
|
|
|
@@ -247,11 +243,7 @@ def plot_euler_angles(
|
|
|
247
243
|
for i in range(4):
|
|
248
244
|
scene_name = f"scene{i + 1}" if i > 0 else "scene"
|
|
249
245
|
fig.update_layout(
|
|
250
|
-
**{
|
|
251
|
-
scene_name: dict(
|
|
252
|
-
aspectmode="cube", camera=dict(eye=dict(x=1.5, y=1.5, z=1.5))
|
|
253
|
-
)
|
|
254
|
-
}
|
|
246
|
+
**{scene_name: dict(aspectmode="cube", camera=dict(eye=dict(x=1.5, y=1.5, z=1.5)))}
|
|
255
247
|
)
|
|
256
248
|
|
|
257
249
|
return fig
|
|
@@ -378,9 +370,7 @@ def plot_quaternion_interpolation(
|
|
|
378
370
|
method="animate",
|
|
379
371
|
args=[
|
|
380
372
|
[None],
|
|
381
|
-
dict(
|
|
382
|
-
frame=dict(duration=0, redraw=False), mode="immediate"
|
|
383
|
-
),
|
|
373
|
+
dict(frame=dict(duration=0, redraw=False), mode="immediate"),
|
|
384
374
|
],
|
|
385
375
|
),
|
|
386
376
|
],
|
pytcl/plotting/ellipses.py
CHANGED
|
@@ -5,10 +5,13 @@ This module provides functions for visualizing uncertainty as ellipses
|
|
|
5
5
|
in 2D and 3D spaces, commonly used in tracking and estimation applications.
|
|
6
6
|
"""
|
|
7
7
|
|
|
8
|
-
from typing import List
|
|
8
|
+
from typing import List
|
|
9
|
+
from typing import Optional
|
|
10
|
+
from typing import Tuple
|
|
9
11
|
|
|
10
12
|
import numpy as np
|
|
11
|
-
from numpy.typing import ArrayLike
|
|
13
|
+
from numpy.typing import ArrayLike
|
|
14
|
+
from numpy.typing import NDArray
|
|
12
15
|
|
|
13
16
|
try:
|
|
14
17
|
import plotly.graph_objects as go
|
pytcl/plotting/metrics.py
CHANGED
|
@@ -5,7 +5,8 @@ This module provides functions for visualizing tracking and estimation
|
|
|
5
5
|
performance metrics such as RMSE, NEES, NIS, and OSPA.
|
|
6
6
|
"""
|
|
7
7
|
|
|
8
|
-
from typing import List
|
|
8
|
+
from typing import List
|
|
9
|
+
from typing import Optional
|
|
9
10
|
|
|
10
11
|
import numpy as np
|
|
11
12
|
from numpy.typing import ArrayLike
|
|
@@ -148,9 +149,7 @@ def plot_nees_sequence(
|
|
|
148
149
|
fig.add_trace(
|
|
149
150
|
go.Scatter(
|
|
150
151
|
x=np.concatenate([time, time[::-1]]),
|
|
151
|
-
y=np.concatenate(
|
|
152
|
-
[np.full(n_steps, upper_bound), np.full(n_steps, lower_bound)]
|
|
153
|
-
),
|
|
152
|
+
y=np.concatenate([np.full(n_steps, upper_bound), np.full(n_steps, lower_bound)]),
|
|
154
153
|
fill="toself",
|
|
155
154
|
fillcolor="rgba(0, 255, 0, 0.1)",
|
|
156
155
|
line=dict(color="rgba(0,0,0,0)"),
|
|
@@ -534,9 +533,7 @@ def plot_consistency_summary(
|
|
|
534
533
|
fig.add_trace(
|
|
535
534
|
go.Scatter(
|
|
536
535
|
x=np.concatenate([time, time[::-1]]),
|
|
537
|
-
y=np.concatenate(
|
|
538
|
-
[np.full(n_steps, nees_upper), np.full(n_steps, nees_lower)]
|
|
539
|
-
),
|
|
536
|
+
y=np.concatenate([np.full(n_steps, nees_upper), np.full(n_steps, nees_lower)]),
|
|
540
537
|
fill="toself",
|
|
541
538
|
fillcolor="rgba(0, 255, 0, 0.1)",
|
|
542
539
|
line=dict(color="rgba(0,0,0,0)"),
|
|
@@ -580,9 +577,7 @@ def plot_consistency_summary(
|
|
|
580
577
|
fig.add_trace(
|
|
581
578
|
go.Scatter(
|
|
582
579
|
x=np.concatenate([time, time[::-1]]),
|
|
583
|
-
y=np.concatenate(
|
|
584
|
-
[np.full(n_steps, nis_upper), np.full(n_steps, nis_lower)]
|
|
585
|
-
),
|
|
580
|
+
y=np.concatenate([np.full(n_steps, nis_upper), np.full(n_steps, nis_lower)]),
|
|
586
581
|
fill="toself",
|
|
587
582
|
fillcolor="rgba(0, 255, 0, 0.1)",
|
|
588
583
|
line=dict(color="rgba(0,0,0,0)"),
|
pytcl/plotting/tracks.py
CHANGED
|
@@ -5,7 +5,10 @@ This module provides functions for visualizing trajectories, tracks,
|
|
|
5
5
|
measurements, and estimation results in 2D and 3D.
|
|
6
6
|
"""
|
|
7
7
|
|
|
8
|
-
from typing import Any
|
|
8
|
+
from typing import Any
|
|
9
|
+
from typing import Dict
|
|
10
|
+
from typing import List
|
|
11
|
+
from typing import Optional
|
|
9
12
|
|
|
10
13
|
import numpy as np
|
|
11
14
|
from numpy.typing import ArrayLike
|
|
@@ -369,11 +372,7 @@ def plot_multi_target_tracks(
|
|
|
369
372
|
|
|
370
373
|
for idx, (track_id, states) in enumerate(tracks.items()):
|
|
371
374
|
states = np.asarray(states)
|
|
372
|
-
color = (
|
|
373
|
-
colors.get(track_id)
|
|
374
|
-
if colors
|
|
375
|
-
else default_colors[idx % len(default_colors)]
|
|
376
|
-
)
|
|
375
|
+
color = colors.get(track_id) if colors else default_colors[idx % len(default_colors)]
|
|
377
376
|
|
|
378
377
|
fig.add_trace(
|
|
379
378
|
go.Scatter(
|
|
@@ -536,9 +535,7 @@ def plot_estimation_comparison(
|
|
|
536
535
|
|
|
537
536
|
# Error bounds
|
|
538
537
|
if covariances is not None:
|
|
539
|
-
sigma = n_std * np.array(
|
|
540
|
-
[np.sqrt(P[state_idx, state_idx]) for P in covariances]
|
|
541
|
-
)
|
|
538
|
+
sigma = n_std * np.array([np.sqrt(P[state_idx, state_idx]) for P in covariances])
|
|
542
539
|
upper = estimates[:, state_idx] + sigma
|
|
543
540
|
lower = estimates[:, state_idx] - sigma
|
|
544
541
|
|
|
@@ -740,9 +737,7 @@ def create_animated_tracking(
|
|
|
740
737
|
method="animate",
|
|
741
738
|
args=[
|
|
742
739
|
[None],
|
|
743
|
-
dict(
|
|
744
|
-
frame=dict(duration=0, redraw=False), mode="immediate"
|
|
745
|
-
),
|
|
740
|
+
dict(frame=dict(duration=0, redraw=False), mode="immediate"),
|
|
746
741
|
],
|
|
747
742
|
),
|
|
748
743
|
],
|
|
@@ -5,50 +5,46 @@ This module provides methods for static (batch) estimation including
|
|
|
5
5
|
least squares variants and robust estimation techniques.
|
|
6
6
|
"""
|
|
7
7
|
|
|
8
|
-
from pytcl.static_estimation.least_squares import
|
|
9
|
-
|
|
10
|
-
|
|
11
|
-
|
|
12
|
-
|
|
13
|
-
|
|
14
|
-
|
|
15
|
-
|
|
16
|
-
|
|
17
|
-
|
|
18
|
-
|
|
8
|
+
from pytcl.static_estimation.least_squares import LSResult
|
|
9
|
+
from pytcl.static_estimation.least_squares import TLSResult
|
|
10
|
+
from pytcl.static_estimation.least_squares import WLSResult
|
|
11
|
+
from pytcl.static_estimation.least_squares import generalized_least_squares
|
|
12
|
+
from pytcl.static_estimation.least_squares import ordinary_least_squares
|
|
13
|
+
from pytcl.static_estimation.least_squares import recursive_least_squares
|
|
14
|
+
from pytcl.static_estimation.least_squares import ridge_regression
|
|
15
|
+
from pytcl.static_estimation.least_squares import total_least_squares
|
|
16
|
+
from pytcl.static_estimation.least_squares import weighted_least_squares
|
|
17
|
+
from pytcl.static_estimation.maximum_likelihood import CRBResult
|
|
18
|
+
from pytcl.static_estimation.maximum_likelihood import MLResult
|
|
19
|
+
from pytcl.static_estimation.maximum_likelihood import aic
|
|
20
|
+
from pytcl.static_estimation.maximum_likelihood import aicc
|
|
21
|
+
from pytcl.static_estimation.maximum_likelihood import bic
|
|
22
|
+
from pytcl.static_estimation.maximum_likelihood import cramer_rao_bound
|
|
23
|
+
from pytcl.static_estimation.maximum_likelihood import cramer_rao_bound_biased
|
|
24
|
+
from pytcl.static_estimation.maximum_likelihood import efficiency
|
|
19
25
|
from pytcl.static_estimation.maximum_likelihood import (
|
|
20
|
-
CRBResult,
|
|
21
|
-
MLResult,
|
|
22
|
-
aic,
|
|
23
|
-
aicc,
|
|
24
|
-
bic,
|
|
25
|
-
cramer_rao_bound,
|
|
26
|
-
cramer_rao_bound_biased,
|
|
27
|
-
efficiency,
|
|
28
26
|
fisher_information_exponential_family,
|
|
29
|
-
fisher_information_gaussian,
|
|
30
|
-
fisher_information_numerical,
|
|
31
|
-
mle_gaussian,
|
|
32
|
-
mle_newton_raphson,
|
|
33
|
-
mle_scoring,
|
|
34
|
-
observed_fisher_information,
|
|
35
|
-
)
|
|
36
|
-
from pytcl.static_estimation.robust import (
|
|
37
|
-
RANSACResult,
|
|
38
|
-
RobustResult,
|
|
39
|
-
cauchy_weight,
|
|
40
|
-
huber_regression,
|
|
41
|
-
huber_rho,
|
|
42
|
-
huber_weight,
|
|
43
|
-
irls,
|
|
44
|
-
mad,
|
|
45
|
-
ransac,
|
|
46
|
-
ransac_n_trials,
|
|
47
|
-
tau_scale,
|
|
48
|
-
tukey_regression,
|
|
49
|
-
tukey_rho,
|
|
50
|
-
tukey_weight,
|
|
51
27
|
)
|
|
28
|
+
from pytcl.static_estimation.maximum_likelihood import fisher_information_gaussian
|
|
29
|
+
from pytcl.static_estimation.maximum_likelihood import fisher_information_numerical
|
|
30
|
+
from pytcl.static_estimation.maximum_likelihood import mle_gaussian
|
|
31
|
+
from pytcl.static_estimation.maximum_likelihood import mle_newton_raphson
|
|
32
|
+
from pytcl.static_estimation.maximum_likelihood import mle_scoring
|
|
33
|
+
from pytcl.static_estimation.maximum_likelihood import observed_fisher_information
|
|
34
|
+
from pytcl.static_estimation.robust import RANSACResult
|
|
35
|
+
from pytcl.static_estimation.robust import RobustResult
|
|
36
|
+
from pytcl.static_estimation.robust import cauchy_weight
|
|
37
|
+
from pytcl.static_estimation.robust import huber_regression
|
|
38
|
+
from pytcl.static_estimation.robust import huber_rho
|
|
39
|
+
from pytcl.static_estimation.robust import huber_weight
|
|
40
|
+
from pytcl.static_estimation.robust import irls
|
|
41
|
+
from pytcl.static_estimation.robust import mad
|
|
42
|
+
from pytcl.static_estimation.robust import ransac
|
|
43
|
+
from pytcl.static_estimation.robust import ransac_n_trials
|
|
44
|
+
from pytcl.static_estimation.robust import tau_scale
|
|
45
|
+
from pytcl.static_estimation.robust import tukey_regression
|
|
46
|
+
from pytcl.static_estimation.robust import tukey_rho
|
|
47
|
+
from pytcl.static_estimation.robust import tukey_weight
|
|
52
48
|
|
|
53
49
|
__all__ = [
|
|
54
50
|
# Least squares results
|
|
@@ -12,10 +12,12 @@ References
|
|
|
12
12
|
Johns Hopkins University Press, 2013.
|
|
13
13
|
"""
|
|
14
14
|
|
|
15
|
-
from typing import NamedTuple
|
|
15
|
+
from typing import NamedTuple
|
|
16
|
+
from typing import Optional
|
|
16
17
|
|
|
17
18
|
import numpy as np
|
|
18
|
-
from numpy.typing import ArrayLike
|
|
19
|
+
from numpy.typing import ArrayLike
|
|
20
|
+
from numpy.typing import NDArray
|
|
19
21
|
|
|
20
22
|
|
|
21
23
|
class LSResult(NamedTuple):
|
|
@@ -303,8 +305,7 @@ def total_least_squares(
|
|
|
303
305
|
# The solution exists if V[n, n] != 0
|
|
304
306
|
if abs(V[n, n]) < 1e-14:
|
|
305
307
|
raise ValueError(
|
|
306
|
-
"TLS solution does not exist. The smallest singular value "
|
|
307
|
-
"has multiplicity > 1."
|
|
308
|
+
"TLS solution does not exist. The smallest singular value " "has multiplicity > 1."
|
|
308
309
|
)
|
|
309
310
|
|
|
310
311
|
# TLS solution: x = -V[0:n, n] / V[n, n]
|
|
@@ -12,10 +12,13 @@ References
|
|
|
12
12
|
Wiley, 2001.
|
|
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
|
-
from numpy.typing import ArrayLike
|
|
20
|
+
from numpy.typing import ArrayLike
|
|
21
|
+
from numpy.typing import NDArray
|
|
19
22
|
|
|
20
23
|
|
|
21
24
|
class MLResult(NamedTuple):
|
|
@@ -654,9 +657,9 @@ def mle_gaussian(
|
|
|
654
657
|
theta = np.array(theta)
|
|
655
658
|
|
|
656
659
|
# Log-likelihood
|
|
657
|
-
log_lik = -n / 2 * np.log(2 * np.pi * var_mle) - np.sum(
|
|
658
|
-
|
|
659
|
-
)
|
|
660
|
+
log_lik = -n / 2 * np.log(2 * np.pi * var_mle) - np.sum((data - mean_mle) ** 2) / (
|
|
661
|
+
2 * var_mle
|
|
662
|
+
)
|
|
660
663
|
|
|
661
664
|
# Fisher information
|
|
662
665
|
n_params = len(theta)
|
|
@@ -12,10 +12,13 @@ References
|
|
|
12
12
|
Cartography," Communications of the ACM, 1981.
|
|
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
|
-
from numpy.typing import ArrayLike
|
|
20
|
+
from numpy.typing import ArrayLike
|
|
21
|
+
from numpy.typing import NDArray
|
|
19
22
|
|
|
20
23
|
|
|
21
24
|
class RobustResult(NamedTuple):
|