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
|
@@ -423,6 +423,25 @@ def fixed_interval_smoother(
|
|
|
423
423
|
result : RTSResult
|
|
424
424
|
Smoothed estimates over the interval.
|
|
425
425
|
|
|
426
|
+
Examples
|
|
427
|
+
--------
|
|
428
|
+
>>> import numpy as np
|
|
429
|
+
>>> # 1D constant velocity model
|
|
430
|
+
>>> x0 = np.array([0.0, 1.0]) # [position, velocity]
|
|
431
|
+
>>> P0 = np.eye(2) * 5.0
|
|
432
|
+
>>> F = np.array([[1, 1], [0, 1]])
|
|
433
|
+
>>> Q = np.array([[0.25, 0.5], [0.5, 1.0]]) * 0.01
|
|
434
|
+
>>> H = np.array([[1, 0]])
|
|
435
|
+
>>> R = np.array([[0.5]])
|
|
436
|
+
>>> measurements = [np.array([0.9]), np.array([2.1]), np.array([3.0]),
|
|
437
|
+
... np.array([4.2]), np.array([4.9])]
|
|
438
|
+
>>> result = fixed_interval_smoother(x0, P0, measurements, F, Q, H, R)
|
|
439
|
+
>>> len(result.x_smooth)
|
|
440
|
+
5
|
|
441
|
+
>>> # Smoothed estimates have lower uncertainty
|
|
442
|
+
>>> np.trace(result.P_smooth[2]) < np.trace(result.P_filt[2])
|
|
443
|
+
True
|
|
444
|
+
|
|
426
445
|
See Also
|
|
427
446
|
--------
|
|
428
447
|
rts_smoother : Full RTS smoother with time-varying parameters.
|
|
@@ -478,6 +497,26 @@ def two_filter_smoother(
|
|
|
478
497
|
result : RTSResult
|
|
479
498
|
Smoothed estimates.
|
|
480
499
|
|
|
500
|
+
Examples
|
|
501
|
+
--------
|
|
502
|
+
>>> import numpy as np
|
|
503
|
+
>>> # 1D position-velocity system
|
|
504
|
+
>>> x0_fwd = np.array([0.0, 1.0])
|
|
505
|
+
>>> P0_fwd = np.eye(2) * 5.0
|
|
506
|
+
>>> # Backward filter starts with diffuse (high uncertainty) prior
|
|
507
|
+
>>> x0_bwd = np.array([5.0, 1.0]) # approximate final state
|
|
508
|
+
>>> P0_bwd = np.eye(2) * 100.0 # diffuse prior
|
|
509
|
+
>>> F = np.array([[1, 1], [0, 1]])
|
|
510
|
+
>>> Q = np.eye(2) * 0.1
|
|
511
|
+
>>> H = np.array([[1, 0]])
|
|
512
|
+
>>> R = np.array([[1.0]])
|
|
513
|
+
>>> measurements = [np.array([0.8]), np.array([1.9]), np.array([3.1]),
|
|
514
|
+
... np.array([4.0]), np.array([5.2])]
|
|
515
|
+
>>> result = two_filter_smoother(x0_fwd, P0_fwd, x0_bwd, P0_bwd,
|
|
516
|
+
... measurements, F, Q, H, R)
|
|
517
|
+
>>> len(result.x_smooth)
|
|
518
|
+
5
|
|
519
|
+
|
|
481
520
|
Notes
|
|
482
521
|
-----
|
|
483
522
|
The two-filter smoother runs two independent Kalman filters:
|
|
@@ -645,6 +684,25 @@ def rts_smoother_single_step(
|
|
|
645
684
|
-------
|
|
646
685
|
result : SmoothedState
|
|
647
686
|
Smoothed state and covariance at current time.
|
|
687
|
+
|
|
688
|
+
Examples
|
|
689
|
+
--------
|
|
690
|
+
>>> import numpy as np
|
|
691
|
+
>>> # After running forward filter and getting smoothed estimate at k+1
|
|
692
|
+
>>> x_filt = np.array([1.0, 0.5]) # filtered state at k
|
|
693
|
+
>>> P_filt = np.eye(2) * 0.5 # filtered covariance at k
|
|
694
|
+
>>> x_pred_next = np.array([1.5, 0.5]) # predicted state at k+1
|
|
695
|
+
>>> P_pred_next = np.eye(2) * 0.6 # predicted covariance at k+1
|
|
696
|
+
>>> x_smooth_next = np.array([1.4, 0.6]) # smoothed state at k+1
|
|
697
|
+
>>> P_smooth_next = np.eye(2) * 0.3 # smoothed covariance at k+1
|
|
698
|
+
>>> F = np.array([[1, 1], [0, 1]])
|
|
699
|
+
>>> result = rts_smoother_single_step(x_filt, P_filt, x_pred_next,
|
|
700
|
+
... P_pred_next, x_smooth_next,
|
|
701
|
+
... P_smooth_next, F)
|
|
702
|
+
>>> result.x.shape
|
|
703
|
+
(2,)
|
|
704
|
+
>>> result.P.shape
|
|
705
|
+
(2, 2)
|
|
648
706
|
"""
|
|
649
707
|
x_s, P_s = kf_smooth(
|
|
650
708
|
x_filt, P_filt, x_pred_next, P_pred_next, x_smooth_next, P_smooth_next, F
|
|
@@ -79,6 +79,13 @@ def drift_constant_acceleration(
|
|
|
79
79
|
-------
|
|
80
80
|
a : ndarray
|
|
81
81
|
Drift vector.
|
|
82
|
+
|
|
83
|
+
Examples
|
|
84
|
+
--------
|
|
85
|
+
>>> x = np.array([0, 10, 1]) # 1D: pos=0, vel=10, acc=1
|
|
86
|
+
>>> a = drift_constant_acceleration(x, num_dims=1)
|
|
87
|
+
>>> a # [vel, acc, 0]
|
|
88
|
+
array([10., 1., 0.])
|
|
82
89
|
"""
|
|
83
90
|
x = np.asarray(x, dtype=np.float64)
|
|
84
91
|
n = 3 # states per dimension
|
|
@@ -124,6 +131,13 @@ def drift_singer(
|
|
|
124
131
|
-----
|
|
125
132
|
The Singer model has acceleration following a first-order Markov process:
|
|
126
133
|
da/dt = -a/tau + w(t)
|
|
134
|
+
|
|
135
|
+
Examples
|
|
136
|
+
--------
|
|
137
|
+
>>> x = np.array([0, 10, 2]) # 1D: pos=0, vel=10, acc=2
|
|
138
|
+
>>> a = drift_singer(x, tau=5.0, num_dims=1)
|
|
139
|
+
>>> a # [vel, acc, -acc/tau]
|
|
140
|
+
array([10. , 2. , -0.4])
|
|
127
141
|
"""
|
|
128
142
|
x = np.asarray(x, dtype=np.float64)
|
|
129
143
|
n = 3 # states per dimension
|
|
@@ -167,6 +181,14 @@ def drift_coordinated_turn_2d(
|
|
|
167
181
|
dy/dt = vy
|
|
168
182
|
dvy/dt = omega * vx
|
|
169
183
|
domega/dt = 0
|
|
184
|
+
|
|
185
|
+
Examples
|
|
186
|
+
--------
|
|
187
|
+
>>> # Aircraft at origin with velocity [100, 0] and turn rate 0.1 rad/s
|
|
188
|
+
>>> x = np.array([0, 100, 0, 0, 0.1])
|
|
189
|
+
>>> a = drift_coordinated_turn_2d(x)
|
|
190
|
+
>>> a # [vx, -omega*vy, vy, omega*vx, 0]
|
|
191
|
+
array([ 100., -0., 0., 10., 0.])
|
|
170
192
|
"""
|
|
171
193
|
x = np.asarray(x, dtype=np.float64)
|
|
172
194
|
pos_x, vel_x, pos_y, vel_y, omega = x
|
|
@@ -209,6 +231,18 @@ def diffusion_constant_velocity(
|
|
|
209
231
|
-------
|
|
210
232
|
D : ndarray
|
|
211
233
|
Diffusion matrix (noise enters through velocity derivative).
|
|
234
|
+
|
|
235
|
+
Examples
|
|
236
|
+
--------
|
|
237
|
+
>>> x = np.array([0, 1, 0, 2]) # 2D state (not used)
|
|
238
|
+
>>> D = diffusion_constant_velocity(x, sigma_a=0.5, num_dims=2)
|
|
239
|
+
>>> D.shape
|
|
240
|
+
(4, 2)
|
|
241
|
+
>>> D # Noise enters velocity states only
|
|
242
|
+
array([[0. , 0. ],
|
|
243
|
+
[0.5, 0. ],
|
|
244
|
+
[0. , 0. ],
|
|
245
|
+
[0. , 0.5]])
|
|
212
246
|
"""
|
|
213
247
|
n = 2 * num_dims
|
|
214
248
|
D = np.zeros((n, num_dims), dtype=np.float64)
|
|
@@ -244,6 +278,17 @@ def diffusion_constant_acceleration(
|
|
|
244
278
|
-------
|
|
245
279
|
D : ndarray
|
|
246
280
|
Diffusion matrix.
|
|
281
|
+
|
|
282
|
+
Examples
|
|
283
|
+
--------
|
|
284
|
+
>>> x = np.array([0, 1, 0.5]) # 1D state (not used)
|
|
285
|
+
>>> D = diffusion_constant_acceleration(x, sigma_j=0.1, num_dims=1)
|
|
286
|
+
>>> D.shape
|
|
287
|
+
(3, 1)
|
|
288
|
+
>>> D # Noise enters acceleration state only
|
|
289
|
+
array([[0. ],
|
|
290
|
+
[0. ],
|
|
291
|
+
[0.1]])
|
|
247
292
|
"""
|
|
248
293
|
n = 3 * num_dims
|
|
249
294
|
D = np.zeros((n, num_dims), dtype=np.float64)
|
|
@@ -286,6 +331,15 @@ def diffusion_singer(
|
|
|
286
331
|
Notes
|
|
287
332
|
-----
|
|
288
333
|
The diffusion coefficient for Singer is sqrt(2*sigma_m^2/tau).
|
|
334
|
+
|
|
335
|
+
Examples
|
|
336
|
+
--------
|
|
337
|
+
>>> x = np.array([0, 1, 0.5]) # 1D state (not used)
|
|
338
|
+
>>> D = diffusion_singer(x, sigma_m=1.0, tau=10.0, num_dims=1)
|
|
339
|
+
>>> D.shape
|
|
340
|
+
(3, 1)
|
|
341
|
+
>>> D[2, 0] # sqrt(2*1^2/10) = sqrt(0.2)
|
|
342
|
+
0.4472135954999579
|
|
289
343
|
"""
|
|
290
344
|
n = 3 * num_dims
|
|
291
345
|
D = np.zeros((n, num_dims), dtype=np.float64)
|
|
@@ -355,6 +409,17 @@ def continuous_to_discrete(
|
|
|
355
409
|
----------
|
|
356
410
|
.. [1] Van Loan, C.F., "Computing Integrals Involving the Matrix
|
|
357
411
|
Exponential", IEEE Trans. Automatic Control, 1978.
|
|
412
|
+
|
|
413
|
+
Examples
|
|
414
|
+
--------
|
|
415
|
+
>>> # 1D constant velocity model
|
|
416
|
+
>>> A = np.array([[0, 1], [0, 0]])
|
|
417
|
+
>>> G = np.array([[0], [1]])
|
|
418
|
+
>>> Q_c = np.array([[1.0]]) # acceleration variance
|
|
419
|
+
>>> F, Q_d = continuous_to_discrete(A, G, Q_c, T=0.1)
|
|
420
|
+
>>> F # State transition matrix
|
|
421
|
+
array([[1. , 0.1],
|
|
422
|
+
[0. , 1. ]])
|
|
358
423
|
"""
|
|
359
424
|
A = np.asarray(A, dtype=np.float64)
|
|
360
425
|
G = np.asarray(G, dtype=np.float64)
|
|
@@ -411,6 +476,19 @@ def discretize_lti(
|
|
|
411
476
|
Discrete-time state transition matrix.
|
|
412
477
|
G : ndarray or None
|
|
413
478
|
Discrete-time input matrix (None if B is None).
|
|
479
|
+
|
|
480
|
+
Examples
|
|
481
|
+
--------
|
|
482
|
+
>>> # 1D constant velocity: dx/dt = v, dv/dt = u (control input)
|
|
483
|
+
>>> A = np.array([[0, 1], [0, 0]])
|
|
484
|
+
>>> B = np.array([[0], [1]])
|
|
485
|
+
>>> F, G = discretize_lti(A, B, T=0.1)
|
|
486
|
+
>>> F # State transition
|
|
487
|
+
array([[1. , 0.1],
|
|
488
|
+
[0. , 1. ]])
|
|
489
|
+
>>> G # Input matrix
|
|
490
|
+
array([[0.005],
|
|
491
|
+
[0.1 ]])
|
|
414
492
|
"""
|
|
415
493
|
A = np.asarray(A, dtype=np.float64)
|
|
416
494
|
n = A.shape[0]
|
|
@@ -454,6 +532,14 @@ def state_jacobian_cv(
|
|
|
454
532
|
-------
|
|
455
533
|
A : ndarray
|
|
456
534
|
Continuous-time state matrix.
|
|
535
|
+
|
|
536
|
+
Examples
|
|
537
|
+
--------
|
|
538
|
+
>>> x = np.array([0, 1]) # 1D state (not used)
|
|
539
|
+
>>> A = state_jacobian_cv(x, num_dims=1)
|
|
540
|
+
>>> A
|
|
541
|
+
array([[0., 1.],
|
|
542
|
+
[0., 0.]])
|
|
457
543
|
"""
|
|
458
544
|
n = 2 # states per dimension
|
|
459
545
|
A_1d = np.array(
|
|
@@ -494,6 +580,15 @@ def state_jacobian_ca(
|
|
|
494
580
|
-------
|
|
495
581
|
A : ndarray
|
|
496
582
|
Continuous-time state matrix.
|
|
583
|
+
|
|
584
|
+
Examples
|
|
585
|
+
--------
|
|
586
|
+
>>> x = np.array([0, 1, 0.5]) # 1D state (not used)
|
|
587
|
+
>>> A = state_jacobian_ca(x, num_dims=1)
|
|
588
|
+
>>> A
|
|
589
|
+
array([[0., 1., 0.],
|
|
590
|
+
[0., 0., 1.],
|
|
591
|
+
[0., 0., 0.]])
|
|
497
592
|
"""
|
|
498
593
|
n = 3 # states per dimension
|
|
499
594
|
A_1d = np.array(
|
|
@@ -538,6 +633,15 @@ def state_jacobian_singer(
|
|
|
538
633
|
-------
|
|
539
634
|
A : ndarray
|
|
540
635
|
Continuous-time state matrix.
|
|
636
|
+
|
|
637
|
+
Examples
|
|
638
|
+
--------
|
|
639
|
+
>>> x = np.array([0, 1, 0.5]) # 1D state (not used)
|
|
640
|
+
>>> A = state_jacobian_singer(x, tau=5.0, num_dims=1)
|
|
641
|
+
>>> A
|
|
642
|
+
array([[ 0. , 1. , 0. ],
|
|
643
|
+
[ 0. , 0. , 1. ],
|
|
644
|
+
[ 0. , 0. , -0.2]])
|
|
541
645
|
"""
|
|
542
646
|
n = 3 # states per dimension
|
|
543
647
|
A_1d = np.array(
|
|
@@ -232,6 +232,12 @@ def f_coord_turn_polar(
|
|
|
232
232
|
- psi is heading angle
|
|
233
233
|
- v is speed magnitude
|
|
234
234
|
- omega is turn rate
|
|
235
|
+
|
|
236
|
+
Examples
|
|
237
|
+
--------
|
|
238
|
+
>>> F = f_coord_turn_polar(T=1.0, omega=0.1, speed=100.0)
|
|
239
|
+
>>> F.shape
|
|
240
|
+
(5, 5)
|
|
235
241
|
"""
|
|
236
242
|
# Handle near-zero turn rate
|
|
237
243
|
if np.abs(omega) < 1e-10:
|
|
@@ -123,6 +123,12 @@ def f_singer_2d(T: float, tau: float) -> NDArray[np.floating]:
|
|
|
123
123
|
F : ndarray
|
|
124
124
|
State transition matrix of shape (6, 6).
|
|
125
125
|
|
|
126
|
+
Examples
|
|
127
|
+
--------
|
|
128
|
+
>>> F = f_singer_2d(T=1.0, tau=10.0)
|
|
129
|
+
>>> F.shape
|
|
130
|
+
(6, 6)
|
|
131
|
+
|
|
126
132
|
See Also
|
|
127
133
|
--------
|
|
128
134
|
f_singer : General Singer model.
|
|
@@ -148,6 +154,12 @@ def f_singer_3d(T: float, tau: float) -> NDArray[np.floating]:
|
|
|
148
154
|
F : ndarray
|
|
149
155
|
State transition matrix of shape (9, 9).
|
|
150
156
|
|
|
157
|
+
Examples
|
|
158
|
+
--------
|
|
159
|
+
>>> F = f_singer_3d(T=1.0, tau=10.0)
|
|
160
|
+
>>> F.shape
|
|
161
|
+
(9, 9)
|
|
162
|
+
|
|
151
163
|
See Also
|
|
152
164
|
--------
|
|
153
165
|
f_singer : General Singer model.
|
|
@@ -1,5 +1,51 @@
|
|
|
1
1
|
"""
|
|
2
2
|
Process noise covariance matrices for coordinated turn models.
|
|
3
|
+
|
|
4
|
+
This module provides functions to construct process noise covariance matrices
|
|
5
|
+
(Q matrices) for coordinated turn motion models used in target tracking.
|
|
6
|
+
Coordinated turn models describe targets that maintain a constant turn rate,
|
|
7
|
+
such as aircraft executing banking maneuvers.
|
|
8
|
+
|
|
9
|
+
The process noise captures uncertainty in the target's motion, including:
|
|
10
|
+
- Linear acceleration uncertainty along the velocity direction
|
|
11
|
+
- Turn rate (angular velocity) uncertainty
|
|
12
|
+
|
|
13
|
+
Available functions:
|
|
14
|
+
- ``q_coord_turn_2d``: 2D coordinated turn (planar motion)
|
|
15
|
+
- ``q_coord_turn_3d``: 3D coordinated turn (spatial motion)
|
|
16
|
+
- ``q_coord_turn_polar``: Polar/heading-speed representation
|
|
17
|
+
|
|
18
|
+
These Q matrices are designed to work with the corresponding state transition
|
|
19
|
+
matrices in :mod:`pytcl.dynamic_models.coordinated_turn`.
|
|
20
|
+
|
|
21
|
+
Examples
|
|
22
|
+
--------
|
|
23
|
+
Create process noise for a 2D coordinated turn tracker:
|
|
24
|
+
|
|
25
|
+
>>> from pytcl.dynamic_models.process_noise import q_coord_turn_2d
|
|
26
|
+
>>> Q = q_coord_turn_2d(T=0.1, sigma_a=2.0) # 0.1s step, 2 m/s² accel noise
|
|
27
|
+
>>> Q.shape
|
|
28
|
+
(4, 4)
|
|
29
|
+
|
|
30
|
+
With turn rate state estimation:
|
|
31
|
+
|
|
32
|
+
>>> Q = q_coord_turn_2d(T=0.1, sigma_a=2.0, sigma_omega=0.01,
|
|
33
|
+
... state_type='position_velocity_omega')
|
|
34
|
+
>>> Q.shape
|
|
35
|
+
(5, 5)
|
|
36
|
+
|
|
37
|
+
See Also
|
|
38
|
+
--------
|
|
39
|
+
pytcl.dynamic_models.coordinated_turn : State transition matrices
|
|
40
|
+
pytcl.dynamic_models.process_noise.constant_velocity : CV process noise
|
|
41
|
+
pytcl.dynamic_models.process_noise.constant_acceleration : CA process noise
|
|
42
|
+
|
|
43
|
+
References
|
|
44
|
+
----------
|
|
45
|
+
.. [1] Bar-Shalom, Y., Li, X.R., and Kirubarajan, T., "Estimation with
|
|
46
|
+
Applications to Tracking and Navigation", Wiley, 2001.
|
|
47
|
+
.. [2] Blackman, S. and Popoli, R., "Design and Analysis of Modern
|
|
48
|
+
Tracking Systems", Artech House, 1999.
|
|
3
49
|
"""
|
|
4
50
|
|
|
5
51
|
import numpy as np
|
|
@@ -229,6 +229,12 @@ def q_constant_acceleration(
|
|
|
229
229
|
Q : ndarray
|
|
230
230
|
Process noise covariance matrix.
|
|
231
231
|
|
|
232
|
+
Examples
|
|
233
|
+
--------
|
|
234
|
+
>>> Q = q_constant_acceleration(T=1.0, sigma_j=0.1, num_dims=2)
|
|
235
|
+
>>> Q.shape
|
|
236
|
+
(6, 6)
|
|
237
|
+
|
|
232
238
|
See Also
|
|
233
239
|
--------
|
|
234
240
|
f_constant_acceleration : State transition matrix for CA model.
|
|
@@ -1,5 +1,57 @@
|
|
|
1
1
|
"""
|
|
2
2
|
Process noise covariance matrices for Singer acceleration model.
|
|
3
|
+
|
|
4
|
+
This module provides functions to construct process noise covariance matrices
|
|
5
|
+
(Q matrices) for the Singer acceleration motion model. The Singer model
|
|
6
|
+
treats target acceleration as a first-order Gauss-Markov process, making it
|
|
7
|
+
well-suited for tracking maneuvering targets with random accelerations.
|
|
8
|
+
|
|
9
|
+
The Singer model is characterized by:
|
|
10
|
+
- A maneuver time constant (tau) that controls how quickly acceleration
|
|
11
|
+
correlations decay
|
|
12
|
+
- An RMS maneuver level (sigma_m) that sets the expected acceleration magnitude
|
|
13
|
+
- State vector [position, velocity, acceleration] per dimension
|
|
14
|
+
|
|
15
|
+
The model dynamics are:
|
|
16
|
+
da/dt = -a/tau + w(t)
|
|
17
|
+
|
|
18
|
+
where w(t) is white noise with spectral density 2*sigma_m²/tau.
|
|
19
|
+
|
|
20
|
+
Available functions:
|
|
21
|
+
- ``q_singer``: Generic N-dimensional Singer process noise
|
|
22
|
+
- ``q_singer_2d``: 2D Singer model (6x6 state)
|
|
23
|
+
- ``q_singer_3d``: 3D Singer model (9x9 state)
|
|
24
|
+
|
|
25
|
+
These Q matrices are designed to work with the corresponding state transition
|
|
26
|
+
matrices in :mod:`pytcl.dynamic_models.singer`.
|
|
27
|
+
|
|
28
|
+
Examples
|
|
29
|
+
--------
|
|
30
|
+
Create process noise for tracking a maneuvering aircraft:
|
|
31
|
+
|
|
32
|
+
>>> from pytcl.dynamic_models.process_noise import q_singer
|
|
33
|
+
>>> Q = q_singer(T=1.0, tau=20.0, sigma_m=3.0) # tau=20s, 3g maneuvers
|
|
34
|
+
>>> Q.shape
|
|
35
|
+
(3, 3)
|
|
36
|
+
|
|
37
|
+
For 3D tracking:
|
|
38
|
+
|
|
39
|
+
>>> Q = q_singer_3d(T=0.1, tau=10.0, sigma_m=2.0)
|
|
40
|
+
>>> Q.shape
|
|
41
|
+
(9, 9)
|
|
42
|
+
|
|
43
|
+
See Also
|
|
44
|
+
--------
|
|
45
|
+
pytcl.dynamic_models.singer : State transition matrices
|
|
46
|
+
pytcl.dynamic_models.process_noise.constant_acceleration : CA process noise
|
|
47
|
+
|
|
48
|
+
References
|
|
49
|
+
----------
|
|
50
|
+
.. [1] Singer, R.A., "Estimating Optimal Tracking Filter Performance for
|
|
51
|
+
Manned Maneuvering Targets", IEEE Trans. Aerospace and Electronic
|
|
52
|
+
Systems, Vol. AES-6, No. 4, July 1970, pp. 473-483.
|
|
53
|
+
.. [2] Bar-Shalom, Y., Li, X.R., and Kirubarajan, T., "Estimation with
|
|
54
|
+
Applications to Tracking and Navigation", Wiley, 2001, Chapter 6.
|
|
3
55
|
"""
|
|
4
56
|
|
|
5
57
|
import numpy as np
|
pytcl/gravity/clenshaw.py
CHANGED
|
@@ -106,6 +106,17 @@ def clenshaw_sum_order(
|
|
|
106
106
|
Sum of C terms weighted by Legendre functions.
|
|
107
107
|
sum_S : float
|
|
108
108
|
Sum of S terms weighted by Legendre functions.
|
|
109
|
+
|
|
110
|
+
Examples
|
|
111
|
+
--------
|
|
112
|
+
>>> import numpy as np
|
|
113
|
+
>>> C = np.zeros((5, 5))
|
|
114
|
+
>>> S = np.zeros((5, 5))
|
|
115
|
+
>>> C[2, 0] = 1.0 # Only C20 term
|
|
116
|
+
>>> cos_theta, sin_theta = np.cos(np.pi/4), np.sin(np.pi/4)
|
|
117
|
+
>>> sum_C, sum_S = clenshaw_sum_order(0, cos_theta, sin_theta, C, S, 4)
|
|
118
|
+
>>> isinstance(sum_C, float)
|
|
119
|
+
True
|
|
109
120
|
"""
|
|
110
121
|
# Handle edge case
|
|
111
122
|
if m > n_max:
|
|
@@ -187,6 +198,18 @@ def clenshaw_sum_order_derivative(
|
|
|
187
198
|
Derivative of sum_C with respect to theta.
|
|
188
199
|
dsum_S : float
|
|
189
200
|
Derivative of sum_S with respect to theta.
|
|
201
|
+
|
|
202
|
+
Examples
|
|
203
|
+
--------
|
|
204
|
+
>>> import numpy as np
|
|
205
|
+
>>> C = np.zeros((5, 5))
|
|
206
|
+
>>> S = np.zeros((5, 5))
|
|
207
|
+
>>> C[2, 0] = -0.0005 # J2-like term
|
|
208
|
+
>>> cos_theta, sin_theta = np.cos(np.pi/4), np.sin(np.pi/4)
|
|
209
|
+
>>> sum_C, sum_S, dsum_C, dsum_S = clenshaw_sum_order_derivative(
|
|
210
|
+
... 0, cos_theta, sin_theta, C, S, 4)
|
|
211
|
+
>>> len([sum_C, sum_S, dsum_C, dsum_S])
|
|
212
|
+
4
|
|
190
213
|
"""
|
|
191
214
|
if m > n_max:
|
|
192
215
|
return 0.0, 0.0, 0.0, 0.0
|
|
@@ -298,6 +321,19 @@ def clenshaw_geoid(
|
|
|
298
321
|
\\sum_{m=0}^{n} P_n^m(\\sin\\phi) (C_{nm}\\cos m\\lambda + S_{nm}\\sin m\\lambda)
|
|
299
322
|
|
|
300
323
|
The n=0 and n=1 terms are excluded as they represent the reference field.
|
|
324
|
+
|
|
325
|
+
Examples
|
|
326
|
+
--------
|
|
327
|
+
>>> import numpy as np
|
|
328
|
+
>>> C = np.zeros((5, 5))
|
|
329
|
+
>>> S = np.zeros((5, 5))
|
|
330
|
+
>>> C[0, 0] = 1.0
|
|
331
|
+
>>> R = 6.378e6
|
|
332
|
+
>>> GM = 3.986e14
|
|
333
|
+
>>> gamma = 9.81
|
|
334
|
+
>>> N = clenshaw_geoid(0, 0, C, S, R, GM, gamma)
|
|
335
|
+
>>> isinstance(N, float)
|
|
336
|
+
True
|
|
301
337
|
"""
|
|
302
338
|
if n_max is None:
|
|
303
339
|
n_max = C.shape[0] - 1
|
|
@@ -381,6 +417,18 @@ def clenshaw_potential(
|
|
|
381
417
|
-------
|
|
382
418
|
float
|
|
383
419
|
Gravitational potential in m^2/s^2.
|
|
420
|
+
|
|
421
|
+
Examples
|
|
422
|
+
--------
|
|
423
|
+
>>> import numpy as np
|
|
424
|
+
>>> C = np.zeros((5, 5))
|
|
425
|
+
>>> S = np.zeros((5, 5))
|
|
426
|
+
>>> C[0, 0] = 1.0 # Central term only
|
|
427
|
+
>>> R = 6.378e6
|
|
428
|
+
>>> GM = 3.986e14
|
|
429
|
+
>>> V = clenshaw_potential(0, 0, R, C, S, R, GM)
|
|
430
|
+
>>> abs(V - GM/R) / (GM/R) < 0.01 # ~GM/r for central term
|
|
431
|
+
True
|
|
384
432
|
"""
|
|
385
433
|
if n_max is None:
|
|
386
434
|
n_max = C.shape[0] - 1
|
|
@@ -464,6 +512,18 @@ def clenshaw_gravity(
|
|
|
464
512
|
Northward component of gravity disturbance in m/s^2.
|
|
465
513
|
g_lon : float
|
|
466
514
|
Eastward component of gravity disturbance in m/s^2.
|
|
515
|
+
|
|
516
|
+
Examples
|
|
517
|
+
--------
|
|
518
|
+
>>> import numpy as np
|
|
519
|
+
>>> C = np.zeros((5, 5))
|
|
520
|
+
>>> S = np.zeros((5, 5))
|
|
521
|
+
>>> C[0, 0] = 1.0
|
|
522
|
+
>>> R = 6.378e6
|
|
523
|
+
>>> GM = 3.986e14
|
|
524
|
+
>>> g_r, g_lat, g_lon = clenshaw_gravity(0, 0, R, C, S, R, GM)
|
|
525
|
+
>>> g_r < 0 # Gravity points inward
|
|
526
|
+
True
|
|
467
527
|
"""
|
|
468
528
|
if n_max is None:
|
|
469
529
|
n_max = C.shape[0] - 1
|
pytcl/gravity/egm.py
CHANGED
|
@@ -131,6 +131,12 @@ def get_data_dir() -> Path:
|
|
|
131
131
|
-------
|
|
132
132
|
Path
|
|
133
133
|
Path to the data directory.
|
|
134
|
+
|
|
135
|
+
Examples
|
|
136
|
+
--------
|
|
137
|
+
>>> data_dir = get_data_dir()
|
|
138
|
+
>>> str(data_dir).endswith('.pytcl/data') or 'PYTCL_DATA_DIR' in dir()
|
|
139
|
+
True
|
|
134
140
|
"""
|
|
135
141
|
env_dir = os.environ.get("PYTCL_DATA_DIR")
|
|
136
142
|
if env_dir:
|
|
@@ -242,6 +248,16 @@ def create_test_coefficients(n_max: int = 36) -> EGMCoefficients:
|
|
|
242
248
|
-------
|
|
243
249
|
EGMCoefficients
|
|
244
250
|
Test coefficient set.
|
|
251
|
+
|
|
252
|
+
Examples
|
|
253
|
+
--------
|
|
254
|
+
>>> coef = create_test_coefficients(n_max=10)
|
|
255
|
+
>>> coef.n_max
|
|
256
|
+
10
|
|
257
|
+
>>> coef.C[0, 0] # Central term
|
|
258
|
+
1.0
|
|
259
|
+
>>> abs(coef.C[2, 0]) > 0 # J2 term present
|
|
260
|
+
True
|
|
245
261
|
"""
|
|
246
262
|
C = np.zeros((n_max + 1, n_max + 1))
|
|
247
263
|
S = np.zeros((n_max + 1, n_max + 1))
|
|
@@ -496,6 +512,16 @@ def geoid_heights(
|
|
|
496
512
|
-------
|
|
497
513
|
ndarray
|
|
498
514
|
Geoid heights in meters.
|
|
515
|
+
|
|
516
|
+
Examples
|
|
517
|
+
--------
|
|
518
|
+
>>> import numpy as np
|
|
519
|
+
>>> coef = create_test_coefficients(n_max=10)
|
|
520
|
+
>>> lats = np.array([0.0, np.pi/4]) # Equator and 45°N
|
|
521
|
+
>>> lons = np.array([0.0, np.pi/2]) # Prime meridian and 90°E
|
|
522
|
+
>>> heights = geoid_heights(lats, lons, coefficients=coef)
|
|
523
|
+
>>> len(heights)
|
|
524
|
+
2
|
|
499
525
|
"""
|
|
500
526
|
# Load coefficients once
|
|
501
527
|
if coefficients is None:
|
|
@@ -543,6 +569,13 @@ def gravity_disturbance(
|
|
|
543
569
|
-------
|
|
544
570
|
GravityDisturbance
|
|
545
571
|
Gravity disturbance components.
|
|
572
|
+
|
|
573
|
+
Examples
|
|
574
|
+
--------
|
|
575
|
+
>>> coef = create_test_coefficients(n_max=10)
|
|
576
|
+
>>> dist = gravity_disturbance(0, 0, h=0, coefficients=coef)
|
|
577
|
+
>>> isinstance(dist.magnitude, float)
|
|
578
|
+
True
|
|
546
579
|
"""
|
|
547
580
|
if coefficients is None:
|
|
548
581
|
coefficients = load_egm_coefficients(model, n_max)
|
|
@@ -613,6 +646,13 @@ def gravity_anomaly(
|
|
|
613
646
|
-------
|
|
614
647
|
float
|
|
615
648
|
Gravity anomaly in m/s^2 (typically reported in mGal = 1e-5 m/s^2).
|
|
649
|
+
|
|
650
|
+
Examples
|
|
651
|
+
--------
|
|
652
|
+
>>> coef = create_test_coefficients(n_max=10)
|
|
653
|
+
>>> anomaly = gravity_anomaly(0, 0, h=0, coefficients=coef)
|
|
654
|
+
>>> isinstance(anomaly, float)
|
|
655
|
+
True
|
|
616
656
|
"""
|
|
617
657
|
disturbance = gravity_disturbance(lat, lon, h, model, n_max, coefficients)
|
|
618
658
|
|
|
@@ -657,6 +697,13 @@ def deflection_of_vertical(
|
|
|
657
697
|
-----
|
|
658
698
|
Positive xi means the plumb line points more north than the normal.
|
|
659
699
|
Positive eta means the plumb line points more east than the normal.
|
|
700
|
+
|
|
701
|
+
Examples
|
|
702
|
+
--------
|
|
703
|
+
>>> coef = create_test_coefficients(n_max=10)
|
|
704
|
+
>>> xi, eta = deflection_of_vertical(0, 0, coefficients=coef)
|
|
705
|
+
>>> isinstance(xi, float) and isinstance(eta, float)
|
|
706
|
+
True
|
|
660
707
|
"""
|
|
661
708
|
if coefficients is None:
|
|
662
709
|
coefficients = load_egm_coefficients(model, n_max)
|
pytcl/gravity/models.py
CHANGED
|
@@ -267,6 +267,12 @@ def gravity_j2(
|
|
|
267
267
|
-------
|
|
268
268
|
result : GravityResult
|
|
269
269
|
Gravity components and magnitude.
|
|
270
|
+
|
|
271
|
+
Examples
|
|
272
|
+
--------
|
|
273
|
+
>>> result = gravity_j2(0, 0, 0) # At equator, sea level
|
|
274
|
+
>>> abs(result.magnitude - 9.78) < 0.01
|
|
275
|
+
True
|
|
270
276
|
"""
|
|
271
277
|
GM = constants.GM
|
|
272
278
|
a = constants.a
|
|
@@ -328,6 +334,12 @@ def geoid_height_j2(
|
|
|
328
334
|
N : float
|
|
329
335
|
Geoid height (geoid - ellipsoid) in meters.
|
|
330
336
|
|
|
337
|
+
Examples
|
|
338
|
+
--------
|
|
339
|
+
>>> N = geoid_height_j2(0) # At equator
|
|
340
|
+
>>> N > 0 # Equator bulges outward
|
|
341
|
+
True
|
|
342
|
+
|
|
331
343
|
Notes
|
|
332
344
|
-----
|
|
333
345
|
This is a simplified model. For accurate geoid heights,
|
|
@@ -371,6 +383,12 @@ def gravitational_potential(
|
|
|
371
383
|
-------
|
|
372
384
|
U : float
|
|
373
385
|
Gravitational potential (m^2/s^2).
|
|
386
|
+
|
|
387
|
+
Examples
|
|
388
|
+
--------
|
|
389
|
+
>>> U = gravitational_potential(0, 0, 6.4e6) # Near Earth surface
|
|
390
|
+
>>> U < 0 # Potential is negative
|
|
391
|
+
True
|
|
374
392
|
"""
|
|
375
393
|
GM = constants.GM
|
|
376
394
|
a = constants.a
|
|
@@ -415,6 +433,14 @@ def free_air_anomaly(
|
|
|
415
433
|
delta_g : float
|
|
416
434
|
Free-air anomaly in m/s^2 (or mGal if multiplied by 1e5).
|
|
417
435
|
|
|
436
|
+
Examples
|
|
437
|
+
--------
|
|
438
|
+
>>> import numpy as np
|
|
439
|
+
>>> # Observed gravity slightly higher than normal
|
|
440
|
+
>>> delta_g = free_air_anomaly(9.81, np.radians(45), 100)
|
|
441
|
+
>>> isinstance(delta_g, float)
|
|
442
|
+
True
|
|
443
|
+
|
|
418
444
|
Notes
|
|
419
445
|
-----
|
|
420
446
|
The free-air anomaly is the difference between observed gravity
|
|
@@ -452,6 +478,14 @@ def bouguer_anomaly(
|
|
|
452
478
|
delta_g : float
|
|
453
479
|
Bouguer anomaly in m/s^2.
|
|
454
480
|
|
|
481
|
+
Examples
|
|
482
|
+
--------
|
|
483
|
+
>>> import numpy as np
|
|
484
|
+
>>> # Bouguer anomaly at mountain location
|
|
485
|
+
>>> delta_g = bouguer_anomaly(9.81, np.radians(45), 1000)
|
|
486
|
+
>>> isinstance(delta_g, float)
|
|
487
|
+
True
|
|
488
|
+
|
|
455
489
|
Notes
|
|
456
490
|
-----
|
|
457
491
|
The Bouguer anomaly removes the gravitational effect of the
|