nrl-tracker 1.9.1__py3-none-any.whl → 1.9.2__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-1.9.1.dist-info → nrl_tracker-1.9.2.dist-info}/METADATA +4 -4
- {nrl_tracker-1.9.1.dist-info → nrl_tracker-1.9.2.dist-info}/RECORD +60 -59
- pytcl/__init__.py +2 -2
- pytcl/assignment_algorithms/gating.py +18 -0
- pytcl/assignment_algorithms/jpda.py +56 -0
- pytcl/assignment_algorithms/nd_assignment.py +65 -0
- pytcl/assignment_algorithms/network_flow.py +40 -0
- pytcl/astronomical/ephemerides.py +18 -0
- pytcl/astronomical/orbital_mechanics.py +131 -0
- pytcl/atmosphere/ionosphere.py +44 -0
- pytcl/atmosphere/models.py +29 -0
- pytcl/clustering/dbscan.py +9 -0
- pytcl/clustering/gaussian_mixture.py +20 -0
- pytcl/clustering/hierarchical.py +29 -0
- pytcl/clustering/kmeans.py +9 -0
- pytcl/coordinate_systems/conversions/geodetic.py +46 -0
- pytcl/coordinate_systems/conversions/spherical.py +35 -0
- pytcl/coordinate_systems/rotations/rotations.py +147 -0
- pytcl/core/__init__.py +16 -0
- pytcl/core/maturity.py +346 -0
- pytcl/dynamic_estimation/gaussian_sum_filter.py +55 -0
- pytcl/dynamic_estimation/imm.py +29 -0
- pytcl/dynamic_estimation/information_filter.py +64 -0
- pytcl/dynamic_estimation/kalman/extended.py +56 -0
- pytcl/dynamic_estimation/kalman/linear.py +69 -0
- pytcl/dynamic_estimation/kalman/unscented.py +81 -0
- pytcl/dynamic_estimation/particle_filters/bootstrap.py +146 -0
- pytcl/dynamic_estimation/rbpf.py +51 -0
- pytcl/dynamic_estimation/smoothers.py +58 -0
- pytcl/dynamic_models/continuous_time/dynamics.py +104 -0
- pytcl/dynamic_models/discrete_time/coordinated_turn.py +6 -0
- pytcl/dynamic_models/discrete_time/singer.py +12 -0
- pytcl/dynamic_models/process_noise/coordinated_turn.py +46 -0
- pytcl/dynamic_models/process_noise/polynomial.py +6 -0
- pytcl/dynamic_models/process_noise/singer.py +52 -0
- pytcl/gravity/clenshaw.py +60 -0
- pytcl/gravity/egm.py +47 -0
- pytcl/gravity/models.py +34 -0
- pytcl/gravity/spherical_harmonics.py +73 -0
- pytcl/gravity/tides.py +34 -0
- pytcl/mathematical_functions/numerical_integration/quadrature.py +85 -0
- pytcl/mathematical_functions/special_functions/bessel.py +55 -0
- pytcl/mathematical_functions/special_functions/elliptic.py +42 -0
- pytcl/mathematical_functions/special_functions/error_functions.py +49 -0
- pytcl/mathematical_functions/special_functions/gamma_functions.py +43 -0
- pytcl/mathematical_functions/special_functions/lambert_w.py +5 -0
- pytcl/mathematical_functions/special_functions/marcum_q.py +16 -0
- pytcl/navigation/geodesy.py +101 -2
- pytcl/navigation/great_circle.py +71 -0
- pytcl/navigation/rhumb.py +74 -0
- pytcl/performance_evaluation/estimation_metrics.py +70 -0
- pytcl/performance_evaluation/track_metrics.py +30 -0
- pytcl/static_estimation/maximum_likelihood.py +54 -0
- pytcl/static_estimation/robust.py +57 -0
- pytcl/terrain/dem.py +69 -0
- pytcl/terrain/visibility.py +65 -0
- pytcl/trackers/hypothesis.py +65 -0
- {nrl_tracker-1.9.1.dist-info → nrl_tracker-1.9.2.dist-info}/LICENSE +0 -0
- {nrl_tracker-1.9.1.dist-info → nrl_tracker-1.9.2.dist-info}/WHEEL +0 -0
- {nrl_tracker-1.9.1.dist-info → nrl_tracker-1.9.2.dist-info}/top_level.txt +0 -0
|
@@ -95,6 +95,11 @@ def gammainc(a: ArrayLike, x: ArrayLike) -> NDArray[np.floating]:
|
|
|
95
95
|
-----
|
|
96
96
|
This is the CDF of the gamma distribution.
|
|
97
97
|
|
|
98
|
+
Examples
|
|
99
|
+
--------
|
|
100
|
+
>>> gammainc(1, 1) # 1 - exp(-1)
|
|
101
|
+
0.632...
|
|
102
|
+
|
|
98
103
|
See Also
|
|
99
104
|
--------
|
|
100
105
|
scipy.special.gammainc : Regularized lower incomplete gamma function.
|
|
@@ -121,6 +126,11 @@ def gammaincc(a: ArrayLike, x: ArrayLike) -> NDArray[np.floating]:
|
|
|
121
126
|
Q : ndarray
|
|
122
127
|
Values of the regularized upper incomplete gamma function.
|
|
123
128
|
|
|
129
|
+
Examples
|
|
130
|
+
--------
|
|
131
|
+
>>> gammaincc(1, 1) # exp(-1)
|
|
132
|
+
0.367...
|
|
133
|
+
|
|
124
134
|
See Also
|
|
125
135
|
--------
|
|
126
136
|
scipy.special.gammaincc : Regularized upper incomplete gamma function.
|
|
@@ -146,6 +156,11 @@ def gammaincinv(a: ArrayLike, y: ArrayLike) -> NDArray[np.floating]:
|
|
|
146
156
|
x : ndarray
|
|
147
157
|
Values where P(a, x) = y.
|
|
148
158
|
|
|
159
|
+
Examples
|
|
160
|
+
--------
|
|
161
|
+
>>> gammaincinv(1, 0.5) # Median of exponential distribution
|
|
162
|
+
0.693...
|
|
163
|
+
|
|
149
164
|
See Also
|
|
150
165
|
--------
|
|
151
166
|
scipy.special.gammaincinv : Inverse of lower incomplete gamma.
|
|
@@ -169,6 +184,11 @@ def digamma(x: ArrayLike) -> NDArray[np.floating]:
|
|
|
169
184
|
ψ : ndarray
|
|
170
185
|
Values of the digamma function.
|
|
171
186
|
|
|
187
|
+
Examples
|
|
188
|
+
--------
|
|
189
|
+
>>> digamma(1) # -γ (negative Euler-Mascheroni constant)
|
|
190
|
+
-0.577...
|
|
191
|
+
|
|
172
192
|
See Also
|
|
173
193
|
--------
|
|
174
194
|
scipy.special.digamma : Digamma function.
|
|
@@ -195,6 +215,13 @@ def polygamma(n: int, x: ArrayLike) -> NDArray[np.floating]:
|
|
|
195
215
|
ψn : ndarray
|
|
196
216
|
Values of the n-th polygamma function.
|
|
197
217
|
|
|
218
|
+
Examples
|
|
219
|
+
--------
|
|
220
|
+
>>> polygamma(0, 1) # Digamma at 1 = -γ
|
|
221
|
+
-0.577...
|
|
222
|
+
>>> polygamma(1, 1) # Trigamma at 1 = π²/6
|
|
223
|
+
1.644...
|
|
224
|
+
|
|
198
225
|
See Also
|
|
199
226
|
--------
|
|
200
227
|
scipy.special.polygamma : Polygamma function.
|
|
@@ -252,6 +279,12 @@ def betaln(a: ArrayLike, b: ArrayLike) -> NDArray[np.floating]:
|
|
|
252
279
|
lnB : ndarray
|
|
253
280
|
Values of ln(B(a, b)).
|
|
254
281
|
|
|
282
|
+
Examples
|
|
283
|
+
--------
|
|
284
|
+
>>> import numpy as np
|
|
285
|
+
>>> betaln(100, 100) # More stable than log(beta(100, 100))
|
|
286
|
+
-137.74...
|
|
287
|
+
|
|
255
288
|
See Also
|
|
256
289
|
--------
|
|
257
290
|
scipy.special.betaln : Log of beta function.
|
|
@@ -284,6 +317,11 @@ def betainc(a: ArrayLike, b: ArrayLike, x: ArrayLike) -> NDArray[np.floating]:
|
|
|
284
317
|
-----
|
|
285
318
|
This is the CDF of the beta distribution.
|
|
286
319
|
|
|
320
|
+
Examples
|
|
321
|
+
--------
|
|
322
|
+
>>> betainc(1, 1, 0.5) # Uniform distribution CDF at 0.5
|
|
323
|
+
0.5
|
|
324
|
+
|
|
287
325
|
See Also
|
|
288
326
|
--------
|
|
289
327
|
scipy.special.betainc : Regularized incomplete beta function.
|
|
@@ -311,6 +349,11 @@ def betaincinv(a: ArrayLike, b: ArrayLike, y: ArrayLike) -> NDArray[np.floating]
|
|
|
311
349
|
x : ndarray
|
|
312
350
|
Values where I_x(a, b) = y.
|
|
313
351
|
|
|
352
|
+
Examples
|
|
353
|
+
--------
|
|
354
|
+
>>> betaincinv(1, 1, 0.5) # Median of uniform distribution
|
|
355
|
+
0.5
|
|
356
|
+
|
|
314
357
|
See Also
|
|
315
358
|
--------
|
|
316
359
|
scipy.special.betaincinv : Inverse of incomplete beta function.
|
|
@@ -170,6 +170,11 @@ def wright_omega(z: ArrayLike) -> NDArray[np.complexfloating]:
|
|
|
170
170
|
|
|
171
171
|
It is entire (analytic everywhere) unlike the Lambert W function.
|
|
172
172
|
|
|
173
|
+
Examples
|
|
174
|
+
--------
|
|
175
|
+
>>> wright_omega(0) # Omega constant
|
|
176
|
+
(0.5671...+0j)
|
|
177
|
+
|
|
173
178
|
References
|
|
174
179
|
----------
|
|
175
180
|
.. [1] Wright, E.M. (1959). "Solution of the equation z*exp(z) = a".
|
|
@@ -112,6 +112,11 @@ def marcum_q1(
|
|
|
112
112
|
Q : ndarray
|
|
113
113
|
Values of Q_1(a, b).
|
|
114
114
|
|
|
115
|
+
Examples
|
|
116
|
+
--------
|
|
117
|
+
>>> marcum_q1(2, 2)
|
|
118
|
+
0.735...
|
|
119
|
+
|
|
115
120
|
See Also
|
|
116
121
|
--------
|
|
117
122
|
marcum_q : Generalized Marcum Q function.
|
|
@@ -274,6 +279,11 @@ def nuttall_q(
|
|
|
274
279
|
-----
|
|
275
280
|
This is the probability P(X <= b^2) for X ~ chi^2(2, a^2).
|
|
276
281
|
|
|
282
|
+
Examples
|
|
283
|
+
--------
|
|
284
|
+
>>> nuttall_q(2, 2) # 1 - Q_1(2, 2)
|
|
285
|
+
0.264...
|
|
286
|
+
|
|
277
287
|
See Also
|
|
278
288
|
--------
|
|
279
289
|
marcum_q : Marcum Q function.
|
|
@@ -322,6 +332,12 @@ def swerling_detection_probability(
|
|
|
322
332
|
For Swerling 1:
|
|
323
333
|
P_d = exp(-threshold / (2 + 2*n*SNR)) * (1 + n*SNR/...)
|
|
324
334
|
|
|
335
|
+
Examples
|
|
336
|
+
--------
|
|
337
|
+
>>> pd = swerling_detection_probability(10, 1e-6, n_pulses=10, swerling_case=0)
|
|
338
|
+
>>> pd > 0.9 # High probability of detection with 10 dB SNR
|
|
339
|
+
True
|
|
340
|
+
|
|
325
341
|
References
|
|
326
342
|
----------
|
|
327
343
|
.. [1] Swerling, P. (1960). "Probability of Detection for Fluctuating
|
pytcl/navigation/geodesy.py
CHANGED
|
@@ -280,8 +280,17 @@ def geodetic_to_ecef(
|
|
|
280
280
|
Examples
|
|
281
281
|
--------
|
|
282
282
|
>>> import numpy as np
|
|
283
|
+
>>> # Philadelphia (40°N, 75°W) at 100m altitude
|
|
283
284
|
>>> lat, lon, alt = np.radians(40.0), np.radians(-75.0), 100.0
|
|
284
285
|
>>> x, y, z = geodetic_to_ecef(lat, lon, alt)
|
|
286
|
+
>>> x / 1e6 # ~1.2 million meters
|
|
287
|
+
1.24...
|
|
288
|
+
>>> # Equator at prime meridian
|
|
289
|
+
>>> x, y, z = geodetic_to_ecef(0.0, 0.0, 0.0)
|
|
290
|
+
>>> x # Semi-major axis (equatorial radius)
|
|
291
|
+
6378137.0
|
|
292
|
+
>>> y, z
|
|
293
|
+
(0.0, 0.0)
|
|
285
294
|
"""
|
|
286
295
|
lat = np.asarray(lat, dtype=np.float64)
|
|
287
296
|
lon = np.asarray(lon, dtype=np.float64)
|
|
@@ -327,6 +336,19 @@ def ecef_to_geodetic(
|
|
|
327
336
|
alt : ndarray
|
|
328
337
|
Altitude above ellipsoid in meters.
|
|
329
338
|
|
|
339
|
+
Examples
|
|
340
|
+
--------
|
|
341
|
+
>>> import numpy as np
|
|
342
|
+
>>> # Point on equator at prime meridian
|
|
343
|
+
>>> lat, lon, alt = ecef_to_geodetic(6378137.0, 0.0, 0.0)
|
|
344
|
+
>>> np.degrees(lat), np.degrees(lon), alt
|
|
345
|
+
(0.0, 0.0, 0.0)
|
|
346
|
+
>>> # Round-trip conversion
|
|
347
|
+
>>> x, y, z = geodetic_to_ecef(np.radians(45.0), np.radians(90.0), 1000.0)
|
|
348
|
+
>>> lat2, lon2, alt2 = ecef_to_geodetic(x, y, z)
|
|
349
|
+
>>> np.degrees(lat2), np.degrees(lon2), alt2
|
|
350
|
+
(45.0..., 90.0..., 1000.0...)
|
|
351
|
+
|
|
330
352
|
Notes
|
|
331
353
|
-----
|
|
332
354
|
Uses Bowring's iterative algorithm for robust conversion.
|
|
@@ -409,12 +431,19 @@ def ecef_to_enu(
|
|
|
409
431
|
|
|
410
432
|
Examples
|
|
411
433
|
--------
|
|
412
|
-
>>>
|
|
434
|
+
>>> import numpy as np
|
|
435
|
+
>>> # Reference point: Philadelphia
|
|
413
436
|
>>> lat_ref, lon_ref, alt_ref = np.radians(40.0), np.radians(-75.0), 0.0
|
|
414
|
-
>>> # Target point
|
|
437
|
+
>>> # Target point slightly east
|
|
415
438
|
>>> lat, lon, alt = np.radians(40.0), np.radians(-74.99), 0.0
|
|
416
439
|
>>> x, y, z = geodetic_to_ecef(lat, lon, alt)
|
|
417
440
|
>>> e, n, u = ecef_to_enu(x, y, z, lat_ref, lon_ref, alt_ref)
|
|
441
|
+
>>> e # East displacement in meters
|
|
442
|
+
850...
|
|
443
|
+
>>> abs(n) < 10 # North displacement should be ~0
|
|
444
|
+
True
|
|
445
|
+
>>> abs(u) < 10 # Up displacement should be ~0
|
|
446
|
+
True
|
|
418
447
|
"""
|
|
419
448
|
x = np.asarray(x, dtype=np.float64)
|
|
420
449
|
y = np.asarray(y, dtype=np.float64)
|
|
@@ -471,6 +500,18 @@ def enu_to_ecef(
|
|
|
471
500
|
-------
|
|
472
501
|
x, y, z : ndarray
|
|
473
502
|
ECEF coordinates in meters.
|
|
503
|
+
|
|
504
|
+
Examples
|
|
505
|
+
--------
|
|
506
|
+
>>> import numpy as np
|
|
507
|
+
>>> # Reference point
|
|
508
|
+
>>> lat_ref, lon_ref, alt_ref = np.radians(40.0), np.radians(-75.0), 0.0
|
|
509
|
+
>>> # 1 km east, 500 m north, 100 m up
|
|
510
|
+
>>> x, y, z = enu_to_ecef(1000.0, 500.0, 100.0, lat_ref, lon_ref, alt_ref)
|
|
511
|
+
>>> # Convert back to verify
|
|
512
|
+
>>> e, n, u = ecef_to_enu(x, y, z, lat_ref, lon_ref, alt_ref)
|
|
513
|
+
>>> e, n, u
|
|
514
|
+
(1000.0..., 500.0..., 100.0...)
|
|
474
515
|
"""
|
|
475
516
|
east = np.asarray(east, dtype=np.float64)
|
|
476
517
|
north = np.asarray(north, dtype=np.float64)
|
|
@@ -522,6 +563,17 @@ def ecef_to_ned(
|
|
|
522
563
|
-------
|
|
523
564
|
north, east, down : ndarray
|
|
524
565
|
NED coordinates in meters relative to reference point.
|
|
566
|
+
|
|
567
|
+
Examples
|
|
568
|
+
--------
|
|
569
|
+
>>> import numpy as np
|
|
570
|
+
>>> # Reference point
|
|
571
|
+
>>> lat_ref, lon_ref, alt_ref = np.radians(40.0), np.radians(-75.0), 0.0
|
|
572
|
+
>>> # Target above reference
|
|
573
|
+
>>> x, y, z = geodetic_to_ecef(lat_ref, lon_ref, 1000.0) # 1km above
|
|
574
|
+
>>> n, e, d = ecef_to_ned(x, y, z, lat_ref, lon_ref, alt_ref)
|
|
575
|
+
>>> abs(n) < 1, abs(e) < 1, d # Should be ~0, ~0, -1000
|
|
576
|
+
(True, True, -1000.0...)
|
|
525
577
|
"""
|
|
526
578
|
east, north, up = ecef_to_enu(x, y, z, lat_ref, lon_ref, alt_ref, ellipsoid)
|
|
527
579
|
return north, east, -up
|
|
@@ -556,6 +608,17 @@ def ned_to_ecef(
|
|
|
556
608
|
-------
|
|
557
609
|
x, y, z : ndarray
|
|
558
610
|
ECEF coordinates in meters.
|
|
611
|
+
|
|
612
|
+
Examples
|
|
613
|
+
--------
|
|
614
|
+
>>> import numpy as np
|
|
615
|
+
>>> lat_ref, lon_ref, alt_ref = np.radians(40.0), np.radians(-75.0), 0.0
|
|
616
|
+
>>> # 100m north, 50m east, 10m down
|
|
617
|
+
>>> x, y, z = ned_to_ecef(100.0, 50.0, 10.0, lat_ref, lon_ref, alt_ref)
|
|
618
|
+
>>> # Verify round-trip
|
|
619
|
+
>>> n, e, d = ecef_to_ned(x, y, z, lat_ref, lon_ref, alt_ref)
|
|
620
|
+
>>> n, e, d
|
|
621
|
+
(100.0..., 50.0..., 10.0...)
|
|
559
622
|
"""
|
|
560
623
|
return enu_to_ecef(
|
|
561
624
|
east, north, -np.asarray(down), lat_ref, lon_ref, alt_ref, ellipsoid
|
|
@@ -597,6 +660,17 @@ def direct_geodetic(
|
|
|
597
660
|
azimuth2 : float
|
|
598
661
|
Back azimuth at destination in radians.
|
|
599
662
|
|
|
663
|
+
Examples
|
|
664
|
+
--------
|
|
665
|
+
>>> import numpy as np
|
|
666
|
+
>>> # From New York, travel 1000 km northeast
|
|
667
|
+
>>> lat1, lon1 = np.radians(40.7), np.radians(-74.0)
|
|
668
|
+
>>> azimuth = np.radians(45) # Northeast
|
|
669
|
+
>>> distance = 1_000_000 # 1000 km
|
|
670
|
+
>>> lat2, lon2, az2 = direct_geodetic(lat1, lon1, azimuth, distance)
|
|
671
|
+
>>> np.degrees(lat2), np.degrees(lon2) # Destination
|
|
672
|
+
(47.0..., -62.6...)
|
|
673
|
+
|
|
600
674
|
References
|
|
601
675
|
----------
|
|
602
676
|
.. [1] Vincenty, T., "Direct and Inverse Solutions of Geodesics on the
|
|
@@ -647,6 +721,18 @@ def inverse_geodetic(
|
|
|
647
721
|
azimuth2 : float
|
|
648
722
|
Back azimuth at destination in radians.
|
|
649
723
|
|
|
724
|
+
Examples
|
|
725
|
+
--------
|
|
726
|
+
>>> import numpy as np
|
|
727
|
+
>>> # Distance from New York to London
|
|
728
|
+
>>> lat1, lon1 = np.radians(40.7128), np.radians(-74.0060) # NYC
|
|
729
|
+
>>> lat2, lon2 = np.radians(51.5074), np.radians(-0.1278) # London
|
|
730
|
+
>>> dist, az1, az2 = inverse_geodetic(lat1, lon1, lat2, lon2)
|
|
731
|
+
>>> dist / 1000 # Distance in km
|
|
732
|
+
5570...
|
|
733
|
+
>>> np.degrees(az1) # Initial heading from NYC
|
|
734
|
+
51.2...
|
|
735
|
+
|
|
650
736
|
Notes
|
|
651
737
|
-----
|
|
652
738
|
May fail to converge for nearly antipodal points.
|
|
@@ -690,6 +776,19 @@ def haversine_distance(
|
|
|
690
776
|
float
|
|
691
777
|
Great-circle distance in meters.
|
|
692
778
|
|
|
779
|
+
Examples
|
|
780
|
+
--------
|
|
781
|
+
>>> import numpy as np
|
|
782
|
+
>>> # Distance from equator to 45°N along prime meridian
|
|
783
|
+
>>> lat1, lon1 = 0.0, 0.0
|
|
784
|
+
>>> lat2, lon2 = np.radians(45.0), 0.0
|
|
785
|
+
>>> dist = haversine_distance(lat1, lon1, lat2, lon2)
|
|
786
|
+
>>> dist / 1000 # ~5000 km
|
|
787
|
+
5003...
|
|
788
|
+
>>> # Same point -> 0 distance
|
|
789
|
+
>>> haversine_distance(0.0, 0.0, 0.0, 0.0)
|
|
790
|
+
0.0
|
|
791
|
+
|
|
693
792
|
Notes
|
|
694
793
|
-----
|
|
695
794
|
This is a spherical approximation. For higher accuracy on an ellipsoid,
|
pytcl/navigation/great_circle.py
CHANGED
|
@@ -260,6 +260,14 @@ def great_circle_inverse(
|
|
|
260
260
|
-------
|
|
261
261
|
GreatCircleResult
|
|
262
262
|
Distance and azimuths.
|
|
263
|
+
|
|
264
|
+
Examples
|
|
265
|
+
--------
|
|
266
|
+
>>> lat1, lon1 = np.radians(40.7128), np.radians(-74.0060) # NYC
|
|
267
|
+
>>> lat2, lon2 = np.radians(51.5074), np.radians(-0.1278) # London
|
|
268
|
+
>>> result = great_circle_inverse(lat1, lon1, lat2, lon2)
|
|
269
|
+
>>> result.distance > 5000000 # Over 5000 km
|
|
270
|
+
True
|
|
263
271
|
"""
|
|
264
272
|
distance = great_circle_distance(lat1, lon1, lat2, lon2, radius)
|
|
265
273
|
azimuth1 = great_circle_azimuth(lat1, lon1, lat2, lon2)
|
|
@@ -346,6 +354,14 @@ def great_circle_waypoints(
|
|
|
346
354
|
-------
|
|
347
355
|
lats, lons : ndarray
|
|
348
356
|
Arrays of waypoint latitudes and longitudes in radians.
|
|
357
|
+
|
|
358
|
+
Examples
|
|
359
|
+
--------
|
|
360
|
+
>>> lat1, lon1 = 0.0, 0.0
|
|
361
|
+
>>> lat2, lon2 = np.pi/4, np.pi/4
|
|
362
|
+
>>> lats, lons = great_circle_waypoints(lat1, lon1, lat2, lon2, 5)
|
|
363
|
+
>>> len(lats)
|
|
364
|
+
5
|
|
349
365
|
"""
|
|
350
366
|
fractions = np.linspace(0, 1, n_points)
|
|
351
367
|
lats = np.zeros(n_points)
|
|
@@ -448,6 +464,16 @@ def cross_track_distance(
|
|
|
448
464
|
-----
|
|
449
465
|
Positive cross-track means the point is to the right of the path
|
|
450
466
|
(when traveling from start to end).
|
|
467
|
+
|
|
468
|
+
Examples
|
|
469
|
+
--------
|
|
470
|
+
>>> # Point near a path from origin to northeast
|
|
471
|
+
>>> lat_pt, lon_pt = np.radians(5), np.radians(2)
|
|
472
|
+
>>> lat1, lon1 = 0.0, 0.0
|
|
473
|
+
>>> lat2, lon2 = np.radians(10), np.radians(10)
|
|
474
|
+
>>> result = cross_track_distance(lat_pt, lon_pt, lat1, lon1, lat2, lon2)
|
|
475
|
+
>>> abs(result.cross_track) < 500000 # Within 500 km
|
|
476
|
+
True
|
|
451
477
|
"""
|
|
452
478
|
# Angular distance from start to point
|
|
453
479
|
d13 = great_circle_distance(lat1, lon1, lat_point, lon_point, radius=1.0)
|
|
@@ -502,6 +528,16 @@ def great_circle_intersect(
|
|
|
502
528
|
Great circles always intersect at two antipodal points (unless they
|
|
503
529
|
are identical or parallel). The returned points are the intersections
|
|
504
530
|
closest to the given points.
|
|
531
|
+
|
|
532
|
+
Examples
|
|
533
|
+
--------
|
|
534
|
+
>>> lat1, lon1 = 0.0, 0.0
|
|
535
|
+
>>> az1 = np.radians(45) # Northeast
|
|
536
|
+
>>> lat2, lon2 = 0.0, np.radians(10)
|
|
537
|
+
>>> az2 = np.radians(315) # Northwest
|
|
538
|
+
>>> result = great_circle_intersect(lat1, lon1, az1, lat2, lon2, az2)
|
|
539
|
+
>>> result.valid
|
|
540
|
+
True
|
|
505
541
|
"""
|
|
506
542
|
|
|
507
543
|
# Convert to Cartesian unit vectors
|
|
@@ -595,6 +631,16 @@ def great_circle_path_intersect(
|
|
|
595
631
|
-------
|
|
596
632
|
IntersectionResult
|
|
597
633
|
Intersection points and validity.
|
|
634
|
+
|
|
635
|
+
Examples
|
|
636
|
+
--------
|
|
637
|
+
>>> # Two crossing paths
|
|
638
|
+
>>> result = great_circle_path_intersect(
|
|
639
|
+
... 0.0, 0.0, np.radians(10), np.radians(10), # Path A
|
|
640
|
+
... 0.0, np.radians(10), np.radians(10), 0.0 # Path B
|
|
641
|
+
... )
|
|
642
|
+
>>> result.valid
|
|
643
|
+
True
|
|
598
644
|
"""
|
|
599
645
|
# Get bearings from start points
|
|
600
646
|
az1 = great_circle_azimuth(lat1a, lon1a, lat2a, lon2a)
|
|
@@ -783,6 +829,22 @@ def angular_distance(
|
|
|
783
829
|
-------
|
|
784
830
|
float
|
|
785
831
|
Angular distance in radians.
|
|
832
|
+
|
|
833
|
+
Examples
|
|
834
|
+
--------
|
|
835
|
+
Compute angular distance between New York and London:
|
|
836
|
+
|
|
837
|
+
>>> import numpy as np
|
|
838
|
+
>>> # NYC: 40.7°N, 74.0°W; London: 51.5°N, 0.1°W
|
|
839
|
+
>>> lat1, lon1 = np.radians(40.7), np.radians(-74.0)
|
|
840
|
+
>>> lat2, lon2 = np.radians(51.5), np.radians(-0.1)
|
|
841
|
+
>>> angle = angular_distance(lat1, lon1, lat2, lon2)
|
|
842
|
+
>>> np.degrees(angle) # about 50 degrees
|
|
843
|
+
49.9...
|
|
844
|
+
|
|
845
|
+
See Also
|
|
846
|
+
--------
|
|
847
|
+
great_circle_distance : Compute distance on sphere with given radius.
|
|
786
848
|
"""
|
|
787
849
|
return great_circle_distance(lat1, lon1, lat2, lon2, radius=1.0)
|
|
788
850
|
|
|
@@ -809,6 +871,15 @@ def destination_point(
|
|
|
809
871
|
-------
|
|
810
872
|
WaypointResult
|
|
811
873
|
Destination coordinates.
|
|
874
|
+
|
|
875
|
+
Examples
|
|
876
|
+
--------
|
|
877
|
+
>>> lat, lon = 0.0, 0.0
|
|
878
|
+
>>> bearing = np.radians(90) # Due East
|
|
879
|
+
>>> ang_dist = np.radians(10) # 10 degrees
|
|
880
|
+
>>> dest = destination_point(lat, lon, bearing, ang_dist)
|
|
881
|
+
>>> np.degrees(dest.lon) # Should be ~10 degrees East
|
|
882
|
+
10.0
|
|
812
883
|
"""
|
|
813
884
|
lat2 = np.arcsin(
|
|
814
885
|
np.sin(lat) * np.cos(angular_distance)
|
pytcl/navigation/rhumb.py
CHANGED
|
@@ -263,6 +263,15 @@ def indirect_rhumb_spherical(
|
|
|
263
263
|
-------
|
|
264
264
|
RhumbResult
|
|
265
265
|
Distance and constant bearing.
|
|
266
|
+
|
|
267
|
+
Examples
|
|
268
|
+
--------
|
|
269
|
+
>>> import numpy as np
|
|
270
|
+
>>> lat1, lon1 = np.radians(40), np.radians(-74) # New York
|
|
271
|
+
>>> lat2, lon2 = np.radians(51), np.radians(0) # London
|
|
272
|
+
>>> result = indirect_rhumb_spherical(lat1, lon1, lat2, lon2)
|
|
273
|
+
>>> result.distance > 5000000 # Over 5000 km
|
|
274
|
+
True
|
|
266
275
|
"""
|
|
267
276
|
distance = rhumb_distance_spherical(lat1, lon1, lat2, lon2, radius)
|
|
268
277
|
bearing = rhumb_bearing(lat1, lon1, lat2, lon2)
|
|
@@ -354,6 +363,15 @@ def rhumb_distance_ellipsoidal(
|
|
|
354
363
|
-------
|
|
355
364
|
float
|
|
356
365
|
Rhumb line distance in meters.
|
|
366
|
+
|
|
367
|
+
Examples
|
|
368
|
+
--------
|
|
369
|
+
>>> import numpy as np
|
|
370
|
+
>>> lat1, lon1 = np.radians(40), np.radians(-74)
|
|
371
|
+
>>> lat2, lon2 = np.radians(51), np.radians(0)
|
|
372
|
+
>>> dist = rhumb_distance_ellipsoidal(lat1, lon1, lat2, lon2)
|
|
373
|
+
>>> dist > 5000000 # Over 5000 km
|
|
374
|
+
True
|
|
357
375
|
"""
|
|
358
376
|
a = ellipsoid.a
|
|
359
377
|
e2 = ellipsoid.e2
|
|
@@ -419,6 +437,15 @@ def indirect_rhumb(
|
|
|
419
437
|
-------
|
|
420
438
|
RhumbResult
|
|
421
439
|
Distance and constant bearing.
|
|
440
|
+
|
|
441
|
+
Examples
|
|
442
|
+
--------
|
|
443
|
+
>>> import numpy as np
|
|
444
|
+
>>> lat1, lon1 = np.radians(40), np.radians(-74) # New York
|
|
445
|
+
>>> lat2, lon2 = np.radians(51), np.radians(0) # London
|
|
446
|
+
>>> result = indirect_rhumb(lat1, lon1, lat2, lon2)
|
|
447
|
+
>>> 0 < result.bearing < np.pi # Eastward bearing
|
|
448
|
+
True
|
|
422
449
|
"""
|
|
423
450
|
distance = rhumb_distance_ellipsoidal(lat1, lon1, lat2, lon2, ellipsoid)
|
|
424
451
|
|
|
@@ -460,6 +487,15 @@ def direct_rhumb(
|
|
|
460
487
|
-------
|
|
461
488
|
RhumbDirectResult
|
|
462
489
|
Destination latitude and longitude.
|
|
490
|
+
|
|
491
|
+
Examples
|
|
492
|
+
--------
|
|
493
|
+
>>> import numpy as np
|
|
494
|
+
>>> lat, lon = np.radians(40), np.radians(-74)
|
|
495
|
+
>>> bearing = np.radians(90) # Due east
|
|
496
|
+
>>> dest = direct_rhumb(lat, lon, bearing, 100000) # 100 km
|
|
497
|
+
>>> np.degrees(dest.lon) > -74 # Moved east
|
|
498
|
+
True
|
|
463
499
|
"""
|
|
464
500
|
a = ellipsoid.a
|
|
465
501
|
e2 = ellipsoid.e2
|
|
@@ -534,6 +570,17 @@ def rhumb_intersect(
|
|
|
534
570
|
RhumbIntersectionResult
|
|
535
571
|
Intersection point and validity flag.
|
|
536
572
|
|
|
573
|
+
Examples
|
|
574
|
+
--------
|
|
575
|
+
>>> import numpy as np
|
|
576
|
+
>>> lat1, lon1 = np.radians(40), np.radians(-74)
|
|
577
|
+
>>> lat2, lon2 = np.radians(51), np.radians(0)
|
|
578
|
+
>>> bearing1 = np.radians(45)
|
|
579
|
+
>>> bearing2 = np.radians(270)
|
|
580
|
+
>>> result = rhumb_intersect(lat1, lon1, bearing1, lat2, lon2, bearing2)
|
|
581
|
+
>>> result.valid # May or may not intersect
|
|
582
|
+
True
|
|
583
|
+
|
|
537
584
|
Notes
|
|
538
585
|
-----
|
|
539
586
|
Unlike great circles, two rhumb lines may not intersect (if bearings
|
|
@@ -612,6 +659,15 @@ def rhumb_midpoint(
|
|
|
612
659
|
-------
|
|
613
660
|
RhumbDirectResult
|
|
614
661
|
Midpoint latitude and longitude.
|
|
662
|
+
|
|
663
|
+
Examples
|
|
664
|
+
--------
|
|
665
|
+
>>> import numpy as np
|
|
666
|
+
>>> lat1, lon1 = np.radians(0), np.radians(0)
|
|
667
|
+
>>> lat2, lon2 = np.radians(10), np.radians(10)
|
|
668
|
+
>>> mid = rhumb_midpoint(lat1, lon1, lat2, lon2)
|
|
669
|
+
>>> np.isclose(np.degrees(mid.lat), 5, atol=0.1)
|
|
670
|
+
True
|
|
615
671
|
"""
|
|
616
672
|
result = indirect_rhumb_spherical(lat1, lon1, lat2, lon2)
|
|
617
673
|
return direct_rhumb_spherical(lat1, lon1, result.bearing, result.distance / 2)
|
|
@@ -643,6 +699,15 @@ def rhumb_waypoints(
|
|
|
643
699
|
-------
|
|
644
700
|
lats, lons : ndarray
|
|
645
701
|
Arrays of waypoint latitudes and longitudes in radians.
|
|
702
|
+
|
|
703
|
+
Examples
|
|
704
|
+
--------
|
|
705
|
+
>>> import numpy as np
|
|
706
|
+
>>> lat1, lon1 = np.radians(40), np.radians(-74)
|
|
707
|
+
>>> lat2, lon2 = np.radians(51), np.radians(0)
|
|
708
|
+
>>> lats, lons = rhumb_waypoints(lat1, lon1, lat2, lon2, 5)
|
|
709
|
+
>>> len(lats)
|
|
710
|
+
5
|
|
646
711
|
"""
|
|
647
712
|
result = indirect_rhumb_spherical(lat1, lon1, lat2, lon2, radius)
|
|
648
713
|
|
|
@@ -685,6 +750,15 @@ def compare_great_circle_rhumb(
|
|
|
685
750
|
Rhumb line distance in meters.
|
|
686
751
|
difference_percent : float
|
|
687
752
|
Percentage difference (rhumb is longer).
|
|
753
|
+
|
|
754
|
+
Examples
|
|
755
|
+
--------
|
|
756
|
+
>>> import numpy as np
|
|
757
|
+
>>> lat1, lon1 = np.radians(40), np.radians(-74) # NYC
|
|
758
|
+
>>> lat2, lon2 = np.radians(51), np.radians(0) # London
|
|
759
|
+
>>> gc, rhumb, diff = compare_great_circle_rhumb(lat1, lon1, lat2, lon2)
|
|
760
|
+
>>> rhumb > gc # Rhumb is always longer
|
|
761
|
+
True
|
|
688
762
|
"""
|
|
689
763
|
from pytcl.navigation.great_circle import great_circle_distance
|
|
690
764
|
|
|
@@ -140,6 +140,14 @@ def velocity_rmse(
|
|
|
140
140
|
-------
|
|
141
141
|
float
|
|
142
142
|
Velocity RMSE.
|
|
143
|
+
|
|
144
|
+
Examples
|
|
145
|
+
--------
|
|
146
|
+
>>> # State = [x, vx, y, vy], velocities are indices [1, 3]
|
|
147
|
+
>>> true = np.array([[0, 10, 0, 5], [1, 10, 0.5, 5]])
|
|
148
|
+
>>> est = np.array([[0, 9.5, 0, 5.2], [1, 10.2, 0.5, 4.9]])
|
|
149
|
+
>>> velocity_rmse(true, est, [1, 3]) # doctest: +SKIP
|
|
150
|
+
0.316...
|
|
143
151
|
"""
|
|
144
152
|
true_vel = true_states[:, velocity_indices]
|
|
145
153
|
est_vel = estimated_states[:, velocity_indices]
|
|
@@ -216,6 +224,15 @@ def nees_sequence(
|
|
|
216
224
|
-------
|
|
217
225
|
ndarray
|
|
218
226
|
NEES values for each time step, shape (N,).
|
|
227
|
+
|
|
228
|
+
Examples
|
|
229
|
+
--------
|
|
230
|
+
>>> true = np.array([[1.0, 2.0], [1.5, 2.5]])
|
|
231
|
+
>>> est = np.array([[1.1, 1.9], [1.6, 2.4]])
|
|
232
|
+
>>> P = np.array([np.eye(2) * 0.1, np.eye(2) * 0.1])
|
|
233
|
+
>>> nees_vals = nees_sequence(true, est, P)
|
|
234
|
+
>>> len(nees_vals)
|
|
235
|
+
2
|
|
219
236
|
"""
|
|
220
237
|
N = true_states.shape[0]
|
|
221
238
|
nees_values = np.zeros(N)
|
|
@@ -247,6 +264,15 @@ def average_nees(
|
|
|
247
264
|
-------
|
|
248
265
|
float
|
|
249
266
|
Average NEES (should be close to state_dim for consistent filter).
|
|
267
|
+
|
|
268
|
+
Examples
|
|
269
|
+
--------
|
|
270
|
+
>>> true = np.array([[1.0, 2.0], [1.5, 2.5], [2.0, 3.0]])
|
|
271
|
+
>>> est = np.array([[1.1, 1.9], [1.6, 2.4], [2.1, 2.9]])
|
|
272
|
+
>>> P = np.array([np.eye(2) * 0.1] * 3)
|
|
273
|
+
>>> avg = average_nees(true, est, P)
|
|
274
|
+
>>> avg # Should be close to state_dim=2 for consistent filter
|
|
275
|
+
0.2
|
|
250
276
|
"""
|
|
251
277
|
return float(np.mean(nees_sequence(true_states, estimated_states, covariances)))
|
|
252
278
|
|
|
@@ -279,6 +305,13 @@ def nis(
|
|
|
279
305
|
|
|
280
306
|
where nu = z - H*x_pred is the innovation and S is the innovation
|
|
281
307
|
covariance.
|
|
308
|
+
|
|
309
|
+
Examples
|
|
310
|
+
--------
|
|
311
|
+
>>> nu = np.array([0.5, -0.3]) # Innovation vector
|
|
312
|
+
>>> S = np.eye(2) * 0.25 # Innovation covariance
|
|
313
|
+
>>> nis(nu, S)
|
|
314
|
+
1.36
|
|
282
315
|
"""
|
|
283
316
|
innovation = np.asarray(innovation)
|
|
284
317
|
innovation_covariance = np.asarray(innovation_covariance)
|
|
@@ -305,6 +338,14 @@ def nis_sequence(
|
|
|
305
338
|
-------
|
|
306
339
|
ndarray
|
|
307
340
|
NIS values for each time step.
|
|
341
|
+
|
|
342
|
+
Examples
|
|
343
|
+
--------
|
|
344
|
+
>>> innovations = np.array([[0.5, -0.3], [0.2, 0.1]])
|
|
345
|
+
>>> S = np.array([np.eye(2) * 0.25, np.eye(2) * 0.25])
|
|
346
|
+
>>> nis_vals = nis_sequence(innovations, S)
|
|
347
|
+
>>> len(nis_vals)
|
|
348
|
+
2
|
|
308
349
|
"""
|
|
309
350
|
N = innovations.shape[0]
|
|
310
351
|
nis_values = np.zeros(N)
|
|
@@ -395,6 +436,15 @@ def credibility_interval(
|
|
|
395
436
|
-------
|
|
396
437
|
float
|
|
397
438
|
Fraction of errors within the interval.
|
|
439
|
+
|
|
440
|
+
Examples
|
|
441
|
+
--------
|
|
442
|
+
>>> rng = np.random.default_rng(42)
|
|
443
|
+
>>> errors = rng.normal(0, 0.1, (100, 2)) # Small errors
|
|
444
|
+
>>> P = np.array([np.eye(2) * 0.1] * 100) # Matching covariance
|
|
445
|
+
>>> frac = credibility_interval(errors, P, interval=0.95)
|
|
446
|
+
>>> frac > 0.9 # Most errors within interval
|
|
447
|
+
True
|
|
398
448
|
"""
|
|
399
449
|
N = len(errors)
|
|
400
450
|
state_dim = errors.shape[1]
|
|
@@ -430,6 +480,16 @@ def monte_carlo_rmse(
|
|
|
430
480
|
-------
|
|
431
481
|
ndarray
|
|
432
482
|
RMSE values.
|
|
483
|
+
|
|
484
|
+
Examples
|
|
485
|
+
--------
|
|
486
|
+
>>> # 3 Monte Carlo runs, 2 time steps, 2 state components
|
|
487
|
+
>>> errors = np.array([[[0.1, 0.2], [0.15, 0.1]],
|
|
488
|
+
... [[0.05, 0.1], [0.2, 0.15]],
|
|
489
|
+
... [[0.15, 0.05], [0.1, 0.2]]])
|
|
490
|
+
>>> rmse_per_time = monte_carlo_rmse(errors, axis=0)
|
|
491
|
+
>>> rmse_per_time.shape
|
|
492
|
+
(2, 2)
|
|
433
493
|
"""
|
|
434
494
|
return np.sqrt(np.mean(errors**2, axis=axis))
|
|
435
495
|
|
|
@@ -453,6 +513,16 @@ def estimation_error_bounds(
|
|
|
453
513
|
ndarray
|
|
454
514
|
Error bounds (standard deviations) for each component,
|
|
455
515
|
shape (N, state_dim).
|
|
516
|
+
|
|
517
|
+
Examples
|
|
518
|
+
--------
|
|
519
|
+
>>> P = np.array([[[1.0, 0], [0, 4.0]],
|
|
520
|
+
... [[0.25, 0], [0, 1.0]]])
|
|
521
|
+
>>> bounds = estimation_error_bounds(P, sigma=2.0)
|
|
522
|
+
>>> bounds[0] # 2-sigma bounds: 2*sqrt(1), 2*sqrt(4)
|
|
523
|
+
array([2., 4.])
|
|
524
|
+
>>> bounds[1] # 2-sigma bounds: 2*sqrt(0.25), 2*sqrt(1)
|
|
525
|
+
array([1., 2.])
|
|
456
526
|
"""
|
|
457
527
|
# Extract diagonal elements (variances)
|
|
458
528
|
variances = np.diagonal(covariances, axis1=1, axis2=2)
|