3D Target Tracking
This example demonstrates tracking targets in 3D space with range-azimuth-elevation measurements.
Overview
3D tracking presents unique challenges:
Spherical measurements: Range, azimuth, and elevation from radar
Coordinate transformations: Converting between measurement and state spaces
3D motion: Constant-velocity filtering of maneuvering targets
Visualization: Displaying tracks in 3D
Key Concepts
Converted-measurement filtering: Spherical radar measurements are transformed to Cartesian before a linear Kalman filter update
RTS smoothing: Batch smoothing of the full 3D trajectory
Multi-sensor fusion: Combining detections from several 3D sensors
Maneuvering targets: Climbing and descending turns tracked with a constant-velocity model
Code Highlights
The example demonstrates:
6-state model: [x, vx, y, vy, z, vz]
Range-azimuth-elevation measurements converted to Cartesian
kf_predict()/kf_update()andrts_smoother()in 3DPlotly 3D visualization of trajectories and estimates
Source Code
1"""
23D Tracking Example
3===================
4
5This example demonstrates target tracking with 3D position measurements
6(x, y, z coordinates). It covers:
7
8State Estimation:
9- 3D constant-velocity Kalman filter
10- Extended Kalman filter for nonlinear measurements
11- RTS smoother for batch processing
12
13Measurement Types:
14- Direct Cartesian (x, y, z) measurements
15- Spherical (range, azimuth, elevation) measurements
16- Multi-sensor fusion in 3D
17
18Applications:
19- Aircraft tracking
20- Spacecraft tracking
21- UAV/drone tracking
22- Marine vessel tracking (with depth)
23
243D tracking extends 2D tracking by adding the z (altitude/depth)
25dimension, which is essential for air and space applications.
26"""
27
28from pathlib import Path
29
30import numpy as np
31import plotly.graph_objects as go
32from plotly.subplots import make_subplots
33
34# Output directory for generated plots
35OUTPUT_DIR = Path(__file__).parent.parent / "docs" / "_static" / "images" / "examples"
36OUTPUT_DIR.mkdir(parents=True, exist_ok=True)
37
38# Global flag to control plotting
39SHOW_PLOTS = True
40
41
42from pytcl.dynamic_estimation import (
43 RTSResult,
44 kf_predict,
45 kf_update,
46 rts_smoother,
47)
48
49
50def generate_3d_cv_trajectory(
51 n_steps: int = 100,
52 dt: float = 1.0,
53 process_noise: float = 0.1,
54 measurement_noise: float = 2.0,
55 seed: int = 42,
56):
57 """Generate a 3D constant-velocity trajectory with measurements.
58
59 The state vector is [x, vx, y, vy, z, vz] (6 states).
60 Measurements are [x, y, z] (3D position).
61
62 Returns:
63 true_states: (n_steps, 6) array of states
64 measurements: list of (3,) measurement arrays [x, y, z]
65 F: state transition matrix (6x6)
66 Q: process noise covariance (6x6)
67 H: measurement matrix (3x6)
68 R: measurement noise covariance (3x3)
69 """
70 rng = np.random.default_rng(seed)
71
72 # State: [x, vx, y, vy, z, vz]
73 # Constant velocity model in 3D
74 F = np.array(
75 [
76 [1, dt, 0, 0, 0, 0],
77 [0, 1, 0, 0, 0, 0],
78 [0, 0, 1, dt, 0, 0],
79 [0, 0, 0, 1, 0, 0],
80 [0, 0, 0, 0, 1, dt],
81 [0, 0, 0, 0, 0, 1],
82 ]
83 )
84
85 # Process noise (discrete white noise acceleration in each axis)
86 q = process_noise
87 Q_1d = (
88 np.array(
89 [
90 [dt**3 / 3, dt**2 / 2],
91 [dt**2 / 2, dt],
92 ]
93 )
94 * q
95 )
96
97 # Build full 6x6 Q matrix
98 Q = np.zeros((6, 6))
99 Q[0:2, 0:2] = Q_1d # x, vx
100 Q[2:4, 2:4] = Q_1d # y, vy
101 Q[4:6, 4:6] = Q_1d # z, vz
102
103 # Measurement: observe 3D position [x, y, z]
104 H = np.array(
105 [
106 [1, 0, 0, 0, 0, 0],
107 [0, 0, 1, 0, 0, 0],
108 [0, 0, 0, 0, 1, 0],
109 ]
110 )
111
112 R = np.eye(3) * measurement_noise**2
113
114 # Generate true trajectory - climbing turn
115 true_states = np.zeros((n_steps, 6))
116 # Start at position (0, 0, 1000) with velocity (50, 30, 5) m/s
117 true_states[0] = [0, 50, 0, 30, 1000, 5]
118
119 for k in range(1, n_steps):
120 # Propagate with process noise
121 process_noise_sample = rng.multivariate_normal(np.zeros(6), Q)
122 true_states[k] = F @ true_states[k - 1] + process_noise_sample
123
124 # Generate measurements
125 measurements = []
126 for k in range(n_steps):
127 meas_noise = rng.multivariate_normal(np.zeros(3), R)
128 z = H @ true_states[k] + meas_noise
129 measurements.append(z)
130
131 return true_states, measurements, F, Q, H, R
132
133
134def demo_3d_kalman_filter():
135 """Demonstrate Kalman filter for 3D tracking."""
136 print("=" * 70)
137 print("3D Kalman Filter Demo")
138 print("=" * 70)
139
140 # Generate 3D trajectory
141 n_steps = 100
142 dt = 1.0
143 true_states, measurements, F, Q, H, R = generate_3d_cv_trajectory(
144 n_steps=n_steps, dt=dt
145 )
146
147 print(f"\nSimulating {n_steps} time steps of 3D tracking")
148 print("State vector: [x, vx, y, vy, z, vz]")
149 print("Measurements: [x, y, z] (3D position)")
150
151 # Initial state estimate
152 x = np.array([0, 0, 0, 0, 1000, 0]) # Unknown velocities
153 P = np.diag([100, 50, 100, 50, 100, 50]) # High initial uncertainty
154
155 # Run Kalman filter
156 estimates = []
157 covariances = []
158
159 for k in range(n_steps):
160 # Predict
161 x_pred = F @ x
162 P_pred = F @ P @ F.T + Q
163
164 # Update
165 z = measurements[k]
166 y = z - H @ x_pred # Innovation
167 S = H @ P_pred @ H.T + R # Innovation covariance
168 K = P_pred @ H.T @ np.linalg.inv(S) # Kalman gain
169
170 x = x_pred + K @ y
171 P = (np.eye(6) - K @ H) @ P_pred
172
173 estimates.append(x.copy())
174 covariances.append(P.copy())
175
176 estimates = np.array(estimates)
177
178 # Compute errors
179 pos_errors = np.sqrt(
180 (estimates[:, 0] - true_states[:, 0]) ** 2
181 + (estimates[:, 2] - true_states[:, 2]) ** 2
182 + (estimates[:, 4] - true_states[:, 4]) ** 2
183 )
184
185 vel_errors = np.sqrt(
186 (estimates[:, 1] - true_states[:, 1]) ** 2
187 + (estimates[:, 3] - true_states[:, 3]) ** 2
188 + (estimates[:, 5] - true_states[:, 5]) ** 2
189 )
190
191 print(f"\nPosition RMSE: {np.sqrt(np.mean(pos_errors**2)):.2f} m")
192 print(f"Velocity RMSE: {np.sqrt(np.mean(vel_errors**2)):.2f} m/s")
193
194 # Show trajectory snapshots
195 print("\nTrajectory snapshots:")
196 print("-" * 70)
197 print(f"{'Time':>6} {'True X':>10} {'True Y':>10} {'True Z':>10} {'3D Error':>10}")
198 print("-" * 70)
199 for t in [0, 25, 50, 75, 99]:
200 true_pos = true_states[t, [0, 2, 4]]
201 print(
202 f"{t:>6} {true_pos[0]:>10.1f} {true_pos[1]:>10.1f} "
203 f"{true_pos[2]:>10.1f} {pos_errors[t]:>10.2f}"
204 )
205
206 # Plot 3D trajectory
207 if SHOW_PLOTS:
208 measurements_arr = np.array(measurements)
209 time = np.arange(n_steps) * dt
210
211 fig = make_subplots(
212 rows=1,
213 cols=3,
214 specs=[[{"type": "scatter3d"}, {"type": "xy"}, {"type": "xy"}]],
215 subplot_titles=(
216 "3D Trajectory",
217 "Position Error Over Time",
218 "XY Projection (Top View)",
219 ),
220 )
221
222 # 3D trajectory plot
223 fig.add_trace(
224 go.Scatter3d(
225 x=true_states[:, 0],
226 y=true_states[:, 2],
227 z=true_states[:, 4],
228 mode="lines",
229 name="True",
230 line=dict(color="blue", width=4),
231 ),
232 row=1,
233 col=1,
234 )
235 fig.add_trace(
236 go.Scatter3d(
237 x=estimates[:, 0],
238 y=estimates[:, 2],
239 z=estimates[:, 4],
240 mode="lines",
241 name="Estimate",
242 line=dict(color="red", width=3, dash="dash"),
243 ),
244 row=1,
245 col=1,
246 )
247 fig.add_trace(
248 go.Scatter3d(
249 x=measurements_arr[::5, 0],
250 y=measurements_arr[::5, 1],
251 z=measurements_arr[::5, 2],
252 mode="markers",
253 name="Measurements",
254 marker=dict(color="gray", size=3, opacity=0.5),
255 ),
256 row=1,
257 col=1,
258 )
259
260 # Position error over time
261 fig.add_trace(
262 go.Scatter(
263 x=time,
264 y=pos_errors,
265 mode="lines",
266 name="Position Error",
267 line=dict(color="blue", width=2),
268 ),
269 row=1,
270 col=2,
271 )
272 fig.add_trace(
273 go.Scatter(
274 x=[time[0], time[-1]],
275 y=[np.mean(pos_errors), np.mean(pos_errors)],
276 mode="lines",
277 name=f"Mean={np.mean(pos_errors):.2f}",
278 line=dict(color="red", width=2, dash="dash"),
279 ),
280 row=1,
281 col=2,
282 )
283
284 # XY projection
285 fig.add_trace(
286 go.Scatter(
287 x=true_states[:, 0],
288 y=true_states[:, 2],
289 mode="lines",
290 name="True",
291 line=dict(color="blue", width=3),
292 showlegend=False,
293 ),
294 row=1,
295 col=3,
296 )
297 fig.add_trace(
298 go.Scatter(
299 x=estimates[:, 0],
300 y=estimates[:, 2],
301 mode="lines",
302 name="Estimate",
303 line=dict(color="red", width=2, dash="dash"),
304 showlegend=False,
305 ),
306 row=1,
307 col=3,
308 )
309 fig.add_trace(
310 go.Scatter(
311 x=measurements_arr[::5, 0],
312 y=measurements_arr[::5, 1],
313 mode="markers",
314 name="Measurements",
315 marker=dict(color="gray", size=4, opacity=0.5),
316 showlegend=False,
317 ),
318 row=1,
319 col=3,
320 )
321
322 fig.update_layout(
323 title="3D Kalman Filter Tracking",
324 height=500,
325 width=1400,
326 showlegend=True,
327 )
328 fig.update_xaxes(title_text="Time (s)", row=1, col=2)
329 fig.update_yaxes(title_text="3D Position Error (m)", row=1, col=2)
330 fig.update_xaxes(title_text="X (m)", row=1, col=3)
331 fig.update_yaxes(title_text="Y (m)", row=1, col=3)
332
333 fig.write_html(
334 str(OUTPUT_DIR / "tracking_3d_kalman.html"),
335 include_plotlyjs="cdn",
336 div_id="tracking_3d_kalman",
337 )
338 print("\n [Plot saved to tracking_3d_kalman.html]")
339
340 return estimates, true_states, measurements
341
342
343def demo_3d_rts_smoother():
344 """Demonstrate RTS smoother for 3D tracking."""
345 print("\n" + "=" * 70)
346 print("3D RTS Smoother Demo")
347 print("=" * 70)
348
349 # Generate 3D trajectory
350 n_steps = 100
351 true_states, measurements, F, Q, H, R = generate_3d_cv_trajectory(n_steps=n_steps)
352
353 # Initial state
354 x0 = np.array([0, 0, 0, 0, 1000, 0])
355 P0 = np.diag([100, 50, 100, 50, 100, 50])
356
357 # Run RTS smoother
358 result = rts_smoother(x0, P0, measurements, F, Q, H, R)
359
360 # Compute errors for filter vs smoother
361 filter_pos_errors = []
362 smooth_pos_errors = []
363
364 for k in range(n_steps):
365 true = true_states[k, [0, 2, 4]] # x, y, z
366
367 filt_pos = result.x_filt[k][[0, 2, 4]]
368 smooth_pos = result.x_smooth[k][[0, 2, 4]]
369
370 filter_pos_errors.append(np.linalg.norm(filt_pos - true))
371 smooth_pos_errors.append(np.linalg.norm(smooth_pos - true))
372
373 print("\n3D Position RMSE comparison:")
374 print(f" Filter: {np.sqrt(np.mean(np.array(filter_pos_errors) ** 2)):.2f} m")
375 print(f" Smoother: {np.sqrt(np.mean(np.array(smooth_pos_errors) ** 2)):.2f} m")
376
377 improvement = (1 - np.mean(smooth_pos_errors) / np.mean(filter_pos_errors)) * 100
378 print(f" Improvement: {improvement:.1f}%")
379
380 # Velocity comparison
381 filter_vel_errors = []
382 smooth_vel_errors = []
383
384 for k in range(n_steps):
385 true = true_states[k, [1, 3, 5]] # vx, vy, vz
386
387 filt_vel = result.x_filt[k][[1, 3, 5]]
388 smooth_vel = result.x_smooth[k][[1, 3, 5]]
389
390 filter_vel_errors.append(np.linalg.norm(filt_vel - true))
391 smooth_vel_errors.append(np.linalg.norm(smooth_vel - true))
392
393 print("\n3D Velocity RMSE comparison:")
394 print(f" Filter: {np.sqrt(np.mean(np.array(filter_vel_errors) ** 2)):.2f} m/s")
395 print(f" Smoother: {np.sqrt(np.mean(np.array(smooth_vel_errors) ** 2)):.2f} m/s")
396
397 vel_improvement = (
398 1 - np.mean(smooth_vel_errors) / np.mean(filter_vel_errors)
399 ) * 100
400 print(f" Improvement: {vel_improvement:.1f}%")
401
402 # Plot comparison
403 if SHOW_PLOTS:
404 smooth_states = np.array(result.x_smooth)
405 time = np.arange(n_steps)
406
407 fig = make_subplots(
408 rows=1,
409 cols=3,
410 specs=[[{"type": "scatter3d"}, {"type": "xy"}, {"type": "xy"}]],
411 subplot_titles=(
412 "3D Smoothed Trajectory",
413 "Filter vs Smoother Error",
414 "Uncertainty Comparison",
415 ),
416 )
417
418 # 3D trajectory comparison
419 fig.add_trace(
420 go.Scatter3d(
421 x=true_states[:, 0],
422 y=true_states[:, 2],
423 z=true_states[:, 4],
424 mode="lines",
425 name="True",
426 line=dict(color="blue", width=4),
427 ),
428 row=1,
429 col=1,
430 )
431 fig.add_trace(
432 go.Scatter3d(
433 x=smooth_states[:, 0],
434 y=smooth_states[:, 2],
435 z=smooth_states[:, 4],
436 mode="lines",
437 name="Smoothed",
438 line=dict(color="red", width=3, dash="dash"),
439 ),
440 row=1,
441 col=1,
442 )
443
444 # Position error comparison
445 fig.add_trace(
446 go.Scatter(
447 x=time,
448 y=filter_pos_errors,
449 mode="lines",
450 name="Filter",
451 line=dict(color="blue", width=2),
452 opacity=0.7,
453 ),
454 row=1,
455 col=2,
456 )
457 fig.add_trace(
458 go.Scatter(
459 x=time,
460 y=smooth_pos_errors,
461 mode="lines",
462 name="Smoother",
463 line=dict(color="red", width=2),
464 opacity=0.7,
465 ),
466 row=1,
467 col=2,
468 )
469
470 # Uncertainty comparison (trace of P)
471 filter_trace = [np.trace(result.P_filt[k]) for k in range(n_steps)]
472 smooth_trace = [np.trace(result.P_smooth[k]) for k in range(n_steps)]
473 fig.add_trace(
474 go.Scatter(
475 x=time,
476 y=filter_trace,
477 mode="lines",
478 name="Filter",
479 line=dict(color="blue", width=2),
480 showlegend=False,
481 ),
482 row=1,
483 col=3,
484 )
485 fig.add_trace(
486 go.Scatter(
487 x=time,
488 y=smooth_trace,
489 mode="lines",
490 name="Smoother",
491 line=dict(color="red", width=2),
492 showlegend=False,
493 ),
494 row=1,
495 col=3,
496 )
497
498 fig.update_layout(
499 title="RTS Smoother Comparison",
500 height=500,
501 width=1400,
502 showlegend=True,
503 )
504 fig.update_xaxes(title_text="Time step", row=1, col=2)
505 fig.update_yaxes(title_text="3D Position Error (m)", row=1, col=2)
506 fig.update_xaxes(title_text="Time step", row=1, col=3)
507 fig.update_yaxes(title_text="Covariance Trace", row=1, col=3)
508
509 fig.write_html(
510 str(OUTPUT_DIR / "tracking_3d_smoother.html"),
511 include_plotlyjs="cdn",
512 div_id="tracking_3d_smoother",
513 )
514 print("\n [Plot saved to tracking_3d_smoother.html]")
515
516
517def demo_spherical_measurements():
518 """Demonstrate 3D tracking with spherical (radar) measurements."""
519 print("\n" + "=" * 70)
520 print("Spherical Measurements (Radar) Demo")
521 print("=" * 70)
522
523 np.random.seed(42)
524
525 # Generate true 3D trajectory (aircraft flight path)
526 n_steps = 80
527 dt = 1.0
528
529 # State: [x, vx, y, vy, z, vz]
530 F = np.array(
531 [
532 [1, dt, 0, 0, 0, 0],
533 [0, 1, 0, 0, 0, 0],
534 [0, 0, 1, dt, 0, 0],
535 [0, 0, 0, 1, 0, 0],
536 [0, 0, 0, 0, 1, dt],
537 [0, 0, 0, 0, 0, 1],
538 ]
539 )
540
541 q = 0.5
542 Q_1d = np.array([[dt**3 / 3, dt**2 / 2], [dt**2 / 2, dt]]) * q
543 Q = np.zeros((6, 6))
544 Q[0:2, 0:2] = Q_1d
545 Q[2:4, 2:4] = Q_1d
546 Q[4:6, 4:6] = Q_1d
547
548 # True trajectory - aircraft at 10km altitude, approaching
549 true_states = np.zeros((n_steps, 6))
550 true_states[0] = [50000, -200, 30000, -100, 10000, 0] # 50km away, approaching
551
552 for k in range(1, n_steps):
553 process_noise = np.random.multivariate_normal(np.zeros(6), Q * 0.1)
554 true_states[k] = F @ true_states[k - 1] + process_noise
555
556 print("\nScenario: Aircraft tracking with radar")
557 print(
558 f" Initial position: ({true_states[0, 0] / 1000:.1f}, "
559 f"{true_states[0, 2] / 1000:.1f}, {true_states[0, 4] / 1000:.1f}) km"
560 )
561 print(
562 f" Final position: ({true_states[-1, 0] / 1000:.1f}, "
563 f"{true_states[-1, 2] / 1000:.1f}, {true_states[-1, 4] / 1000:.1f}) km"
564 )
565
566 # Radar measurement function: Cartesian -> (range, azimuth, elevation)
567 def cartesian_to_spherical(x, y, z):
568 r = np.sqrt(x**2 + y**2 + z**2)
569 az = np.arctan2(y, x)
570 el = np.arctan2(z, np.sqrt(x**2 + y**2))
571 return np.array([r, az, el])
572
573 def spherical_to_cartesian(r, az, el):
574 x = r * np.cos(el) * np.cos(az)
575 y = r * np.cos(el) * np.sin(az)
576 z = r * np.sin(el)
577 return np.array([x, y, z])
578
579 # Measurement noise (typical radar)
580 sigma_r = 50.0 # 50m range noise
581 sigma_az = np.radians(0.5) # 0.5 degree azimuth noise
582 sigma_el = np.radians(0.5) # 0.5 degree elevation noise
583 R_spherical = np.diag([sigma_r**2, sigma_az**2, sigma_el**2])
584
585 # Generate spherical measurements
586 spherical_measurements = []
587 for k in range(n_steps):
588 x, y, z = true_states[k, 0], true_states[k, 2], true_states[k, 4]
589 z_true = cartesian_to_spherical(x, y, z)
590 noise = np.array(
591 [
592 np.random.randn() * sigma_r,
593 np.random.randn() * sigma_az,
594 np.random.randn() * sigma_el,
595 ]
596 )
597 spherical_measurements.append(z_true + noise)
598
599 print(f"\nMeasurement noise:")
600 print(f" Range: {sigma_r} m")
601 print(f" Azimuth: {np.degrees(sigma_az):.2f} deg")
602 print(f" Elevation: {np.degrees(sigma_el):.2f} deg")
603
604 # Convert measurements to Cartesian for simple Kalman filter
605 cartesian_measurements = []
606 for z_sph in spherical_measurements:
607 r, az, el = z_sph
608 cart = spherical_to_cartesian(r, az, el)
609 cartesian_measurements.append(cart)
610
611 # Measurement matrix for Cartesian measurements
612 H = np.array(
613 [
614 [1, 0, 0, 0, 0, 0],
615 [0, 0, 1, 0, 0, 0],
616 [0, 0, 0, 0, 1, 0],
617 ]
618 )
619
620 # Approximate Cartesian measurement noise at mid-range
621 avg_range = np.mean([z[0] for z in spherical_measurements])
622 R_cart = np.diag(
623 [
624 sigma_r**2 + (avg_range * sigma_az) ** 2,
625 sigma_r**2 + (avg_range * sigma_az) ** 2,
626 sigma_r**2 + (avg_range * sigma_el) ** 2,
627 ]
628 )
629
630 # Run Kalman filter
631 x = np.array([50000, 0, 30000, 0, 10000, 0]) # Initial guess
632 P = np.diag([10000, 500, 10000, 500, 5000, 100])
633
634 estimates = []
635 for k in range(n_steps):
636 # Predict
637 x = F @ x
638 P = F @ P @ F.T + Q
639
640 # Update with Cartesian measurement
641 z = cartesian_measurements[k]
642 y = z - H @ x
643 S = H @ P @ H.T + R_cart
644 K = P @ H.T @ np.linalg.inv(S)
645 x = x + K @ y
646 P = (np.eye(6) - K @ H) @ P
647
648 estimates.append(x.copy())
649
650 estimates = np.array(estimates)
651
652 # Compute 3D position errors
653 pos_errors = np.sqrt(
654 (estimates[:, 0] - true_states[:, 0]) ** 2
655 + (estimates[:, 2] - true_states[:, 2]) ** 2
656 + (estimates[:, 4] - true_states[:, 4]) ** 2
657 )
658
659 print(f"\n3D Position RMSE: {np.sqrt(np.mean(pos_errors**2)):.1f} m")
660
661 # Show range over time
662 ranges = [np.sqrt(s[0] ** 2 + s[2] ** 2 + s[4] ** 2) for s in true_states]
663 print(f"\nRange: {ranges[0] / 1000:.1f} km -> {ranges[-1] / 1000:.1f} km")
664
665 # Plot
666 if SHOW_PLOTS:
667 sph_arr = np.array(spherical_measurements)
668 ranges_km = np.array(ranges) / 1000
669
670 fig = make_subplots(
671 rows=1,
672 cols=3,
673 specs=[[{"type": "scatter3d"}, {"type": "xy"}, {"type": "xy"}]],
674 subplot_titles=(
675 "Radar Tracking (3D)",
676 "Error vs Range",
677 "Radar Measurements (Spherical)",
678 ),
679 )
680
681 # 3D trajectory
682 fig.add_trace(
683 go.Scatter3d(
684 x=true_states[:, 0] / 1000,
685 y=true_states[:, 2] / 1000,
686 z=true_states[:, 4] / 1000,
687 mode="lines",
688 name="True",
689 line=dict(color="blue", width=4),
690 ),
691 row=1,
692 col=1,
693 )
694 fig.add_trace(
695 go.Scatter3d(
696 x=estimates[:, 0] / 1000,
697 y=estimates[:, 2] / 1000,
698 z=estimates[:, 4] / 1000,
699 mode="lines",
700 name="Estimate",
701 line=dict(color="red", width=3, dash="dash"),
702 ),
703 row=1,
704 col=1,
705 )
706 fig.add_trace(
707 go.Scatter3d(
708 x=[0],
709 y=[0],
710 z=[0],
711 mode="markers",
712 name="Radar",
713 marker=dict(color="green", size=8, symbol="diamond"),
714 ),
715 row=1,
716 col=1,
717 )
718
719 # Position error vs range
720 fig.add_trace(
721 go.Scatter(
722 x=ranges_km,
723 y=pos_errors,
724 mode="markers",
725 name="Error",
726 marker=dict(color="blue", size=6, opacity=0.6),
727 ),
728 row=1,
729 col=2,
730 )
731
732 # Spherical measurements (Az vs El, colored by range)
733 fig.add_trace(
734 go.Scatter(
735 x=np.degrees(sph_arr[:, 1]),
736 y=np.degrees(sph_arr[:, 2]),
737 mode="markers",
738 name="Measurements",
739 marker=dict(
740 color=sph_arr[:, 0] / 1000,
741 colorscale="Viridis",
742 size=8,
743 colorbar=dict(title="Range (km)", x=1.02),
744 ),
745 ),
746 row=1,
747 col=3,
748 )
749
750 fig.update_layout(
751 title="Spherical (Radar) Measurements Tracking",
752 height=500,
753 width=1400,
754 showlegend=True,
755 )
756 fig.update_xaxes(title_text="Range (km)", row=1, col=2)
757 fig.update_yaxes(title_text="3D Position Error (m)", row=1, col=2)
758 fig.update_xaxes(title_text="Azimuth (deg)", row=1, col=3)
759 fig.update_yaxes(title_text="Elevation (deg)", row=1, col=3)
760
761 fig.write_html(
762 str(OUTPUT_DIR / "tracking_3d_radar.html"),
763 include_plotlyjs="cdn",
764 div_id="tracking_3d_radar",
765 )
766 print("\n [Plot saved to tracking_3d_radar.html]")
767
768
769def demo_multi_sensor_3d():
770 """Demonstrate 3D tracking with multiple sensors."""
771 print("\n" + "=" * 70)
772 print("Multi-Sensor 3D Fusion Demo")
773 print("=" * 70)
774
775 np.random.seed(42)
776
777 # Generate 3D trajectory
778 n_steps = 60
779 dt = 1.0
780
781 F = np.array(
782 [
783 [1, dt, 0, 0, 0, 0],
784 [0, 1, 0, 0, 0, 0],
785 [0, 0, 1, dt, 0, 0],
786 [0, 0, 0, 1, 0, 0],
787 [0, 0, 0, 0, 1, dt],
788 [0, 0, 0, 0, 0, 1],
789 ]
790 )
791
792 q = 0.2
793 Q_1d = np.array([[dt**3 / 3, dt**2 / 2], [dt**2 / 2, dt]]) * q
794 Q = np.zeros((6, 6))
795 Q[0:2, 0:2] = Q_1d
796 Q[2:4, 2:4] = Q_1d
797 Q[4:6, 4:6] = Q_1d
798
799 # True trajectory
800 true_states = np.zeros((n_steps, 6))
801 true_states[0] = [0, 10, 0, 5, 100, 2]
802
803 for k in range(1, n_steps):
804 process_noise = np.random.multivariate_normal(np.zeros(6), Q * 0.1)
805 true_states[k] = F @ true_states[k - 1] + process_noise
806
807 # Define sensors with different noise characteristics
808 sensors = [
809 {"name": "GPS", "noise_std": [3.0, 3.0, 5.0]}, # x, y, z noise in meters
810 {"name": "Radar", "noise_std": [5.0, 5.0, 8.0]},
811 {"name": "Lidar", "noise_std": [0.5, 0.5, 0.8]}, # Most accurate
812 ]
813
814 print("\nSensors:")
815 for s in sensors:
816 print(f" {s['name']}: noise std = {s['noise_std']}")
817
818 # Generate measurements from each sensor
819 sensor_measurements = {s["name"]: [] for s in sensors}
820
821 for k in range(n_steps):
822 true_pos = true_states[k, [0, 2, 4]] # x, y, z
823 for sensor in sensors:
824 noise = np.array(
825 [
826 np.random.randn() * sensor["noise_std"][0],
827 np.random.randn() * sensor["noise_std"][1],
828 np.random.randn() * sensor["noise_std"][2],
829 ]
830 )
831 sensor_measurements[sensor["name"]].append(true_pos + noise)
832
833 H = np.array(
834 [
835 [1, 0, 0, 0, 0, 0],
836 [0, 0, 1, 0, 0, 0],
837 [0, 0, 0, 0, 1, 0],
838 ]
839 )
840
841 # Track using individual sensors and fused
842 results = {}
843
844 for sensor in sensors:
845 R = np.diag([s**2 for s in sensor["noise_std"]])
846 x = np.array([0, 0, 0, 0, 100, 0])
847 P = np.diag([100, 50, 100, 50, 100, 50])
848
849 estimates = []
850 for k in range(n_steps):
851 x = F @ x
852 P = F @ P @ F.T + Q
853
854 z = sensor_measurements[sensor["name"]][k]
855 y = z - H @ x
856 S = H @ P @ H.T + R
857 K = P @ H.T @ np.linalg.inv(S)
858 x = x + K @ y
859 P = (np.eye(6) - K @ H) @ P
860
861 estimates.append(x.copy())
862
863 results[sensor["name"]] = np.array(estimates)
864
865 # Fused tracking (combine all sensor measurements)
866 R_fused = np.zeros((3, 3))
867 for sensor in sensors:
868 R_inv = np.diag([1 / s**2 for s in sensor["noise_std"]])
869 R_fused += R_inv
870 R_fused = np.linalg.inv(R_fused)
871
872 x = np.array([0, 0, 0, 0, 100, 0])
873 P = np.diag([100, 50, 100, 50, 100, 50])
874
875 fused_estimates = []
876 for k in range(n_steps):
877 x = F @ x
878 P = F @ P @ F.T + Q
879
880 # Fuse measurements using information form
881 z_fused = np.zeros(3)
882 for sensor in sensors:
883 R_inv = np.diag([1 / s**2 for s in sensor["noise_std"]])
884 z_fused += R_inv @ sensor_measurements[sensor["name"]][k]
885 z_fused = R_fused @ z_fused
886
887 y = z_fused - H @ x
888 S = H @ P @ H.T + R_fused
889 K = P @ H.T @ np.linalg.inv(S)
890 x = x + K @ y
891 P = (np.eye(6) - K @ H) @ P
892
893 fused_estimates.append(x.copy())
894
895 results["Fused"] = np.array(fused_estimates)
896
897 # Compute RMSE for each
898 print("\n3D Position RMSE:")
899 print("-" * 40)
900 for name, estimates in results.items():
901 errors = np.sqrt(
902 (estimates[:, 0] - true_states[:, 0]) ** 2
903 + (estimates[:, 2] - true_states[:, 2]) ** 2
904 + (estimates[:, 4] - true_states[:, 4]) ** 2
905 )
906 rmse = np.sqrt(np.mean(errors**2))
907 print(f" {name:10s}: {rmse:.3f} m")
908
909 # Plot
910 if SHOW_PLOTS:
911 time = np.arange(n_steps)
912 colors = {"GPS": "blue", "Radar": "orange", "Lidar": "green", "Fused": "red"}
913
914 fig = make_subplots(
915 rows=1,
916 cols=3,
917 specs=[[{"type": "scatter3d"}, {"type": "xy"}, {"type": "xy"}]],
918 subplot_titles=(
919 "Multi-Sensor 3D Tracking",
920 "Position Error Comparison",
921 "XZ Projection (Side View)",
922 ),
923 )
924
925 # 3D trajectory
926 fig.add_trace(
927 go.Scatter3d(
928 x=true_states[:, 0],
929 y=true_states[:, 2],
930 z=true_states[:, 4],
931 mode="lines",
932 name="True",
933 line=dict(color="black", width=4),
934 ),
935 row=1,
936 col=1,
937 )
938 for name in ["GPS", "Fused"]:
939 est = results[name]
940 fig.add_trace(
941 go.Scatter3d(
942 x=est[:, 0],
943 y=est[:, 2],
944 z=est[:, 4],
945 mode="lines",
946 name=name,
947 line=dict(
948 color=colors[name],
949 width=3,
950 dash="dash" if name != "Fused" else "solid",
951 ),
952 ),
953 row=1,
954 col=1,
955 )
956
957 # Error comparison
958 for name, estimates in results.items():
959 errors = np.sqrt(
960 (estimates[:, 0] - true_states[:, 0]) ** 2
961 + (estimates[:, 2] - true_states[:, 2]) ** 2
962 + (estimates[:, 4] - true_states[:, 4]) ** 2
963 )
964 fig.add_trace(
965 go.Scatter(
966 x=time,
967 y=errors,
968 mode="lines",
969 name=name,
970 line=dict(color=colors[name], width=2),
971 opacity=0.8,
972 ),
973 row=1,
974 col=2,
975 )
976
977 # XZ projection (side view)
978 fig.add_trace(
979 go.Scatter(
980 x=true_states[:, 0],
981 y=true_states[:, 4],
982 mode="lines",
983 name="True",
984 line=dict(color="black", width=3),
985 showlegend=False,
986 ),
987 row=1,
988 col=3,
989 )
990 fig.add_trace(
991 go.Scatter(
992 x=results["Fused"][:, 0],
993 y=results["Fused"][:, 4],
994 mode="lines",
995 name="Fused",
996 line=dict(color="red", width=2),
997 showlegend=False,
998 ),
999 row=1,
1000 col=3,
1001 )
1002
1003 fig.update_layout(
1004 title="Multi-Sensor 3D Fusion",
1005 height=500,
1006 width=1400,
1007 showlegend=True,
1008 )
1009 fig.update_xaxes(title_text="Time step", row=1, col=2)
1010 fig.update_yaxes(title_text="3D Position Error (m)", row=1, col=2)
1011 fig.update_xaxes(title_text="X (m)", row=1, col=3)
1012 fig.update_yaxes(title_text="Z (m)", row=1, col=3)
1013
1014 fig.write_html(
1015 str(OUTPUT_DIR / "tracking_3d_multisensor.html"),
1016 include_plotlyjs="cdn",
1017 div_id="tracking_3d_multisensor",
1018 )
1019 print("\n [Plot saved to tracking_3d_multisensor.html]")
1020
1021
1022def demo_3d_maneuvering_target():
1023 """Demonstrate tracking a maneuvering target in 3D."""
1024 print("\n" + "=" * 70)
1025 print("Maneuvering Target Demo")
1026 print("=" * 70)
1027
1028 np.random.seed(42)
1029
1030 n_steps = 120
1031 dt = 1.0
1032
1033 # Generate maneuvering trajectory (includes turns and altitude changes)
1034 true_states = np.zeros((n_steps, 6))
1035 true_states[0] = [0, 50, 0, 0, 1000, 0]
1036
1037 print("\nScenario: Maneuvering aircraft")
1038 print(" Phase 1 (t=0-40): Straight flight")
1039 print(" Phase 2 (t=40-80): Climbing turn")
1040 print(" Phase 3 (t=80-120): Descending turn")
1041
1042 for k in range(1, n_steps):
1043 x, vx, y, vy, z, vz = true_states[k - 1]
1044
1045 if k < 40:
1046 # Straight flight
1047 vx_new, vy_new, vz_new = 50, 0, 0
1048 elif k < 80:
1049 # Climbing turn (increase vy, increase vz)
1050 omega = 0.02 # Turn rate
1051 vx_new = 50 * np.cos(omega * (k - 40))
1052 vy_new = 50 * np.sin(omega * (k - 40))
1053 vz_new = 5 # Climbing
1054 else:
1055 # Descending turn
1056 omega = -0.03
1057 vx_new = 40 * np.cos(omega * (k - 80))
1058 vy_new = 40 * np.sin(omega * (k - 80)) + 30
1059 vz_new = -3 # Descending
1060
1061 # Add process noise
1062 noise = np.random.randn(6) * np.array([1, 0.5, 1, 0.5, 1, 0.5])
1063
1064 true_states[k] = [
1065 x + vx * dt + noise[0],
1066 vx_new + noise[1],
1067 y + vy * dt + noise[2],
1068 vy_new + noise[3],
1069 z + vz * dt + noise[4],
1070 vz_new + noise[5],
1071 ]
1072
1073 # Generate noisy measurements
1074 R = np.diag([4.0, 4.0, 9.0]) # x, y, z measurement noise
1075 H = np.array(
1076 [
1077 [1, 0, 0, 0, 0, 0],
1078 [0, 0, 1, 0, 0, 0],
1079 [0, 0, 0, 0, 1, 0],
1080 ]
1081 )
1082
1083 measurements = []
1084 for k in range(n_steps):
1085 true_pos = true_states[k, [0, 2, 4]]
1086 noise = np.random.multivariate_normal(np.zeros(3), R)
1087 measurements.append(true_pos + noise)
1088
1089 # Constant velocity model (will struggle with maneuvers)
1090 F_cv = np.array(
1091 [
1092 [1, dt, 0, 0, 0, 0],
1093 [0, 1, 0, 0, 0, 0],
1094 [0, 0, 1, dt, 0, 0],
1095 [0, 0, 0, 1, 0, 0],
1096 [0, 0, 0, 0, 1, dt],
1097 [0, 0, 0, 0, 0, 1],
1098 ]
1099 )
1100
1101 # Low process noise (assumes constant velocity)
1102 q_low = 0.1
1103 Q_low = np.zeros((6, 6))
1104 for i in [0, 2, 4]:
1105 Q_low[i : i + 2, i : i + 2] = (
1106 np.array([[dt**3 / 3, dt**2 / 2], [dt**2 / 2, dt]]) * q_low
1107 )
1108
1109 # High process noise (handles maneuvers better)
1110 q_high = 5.0
1111 Q_high = np.zeros((6, 6))
1112 for i in [0, 2, 4]:
1113 Q_high[i : i + 2, i : i + 2] = (
1114 np.array([[dt**3 / 3, dt**2 / 2], [dt**2 / 2, dt]]) * q_high
1115 )
1116
1117 # Track with both process noise levels
1118 def run_filter(Q):
1119 x = np.array([0, 50, 0, 0, 1000, 0])
1120 P = np.diag([100, 50, 100, 50, 100, 50])
1121 estimates = []
1122
1123 for k in range(n_steps):
1124 x = F_cv @ x
1125 P = F_cv @ P @ F_cv.T + Q
1126
1127 z = measurements[k]
1128 y = z - H @ x
1129 S = H @ P @ H.T + R
1130 K = P @ H.T @ np.linalg.inv(S)
1131 x = x + K @ y
1132 P = (np.eye(6) - K @ H) @ P
1133
1134 estimates.append(x.copy())
1135
1136 return np.array(estimates)
1137
1138 est_low = run_filter(Q_low)
1139 est_high = run_filter(Q_high)
1140
1141 # Compute errors
1142 def compute_errors(estimates):
1143 return np.sqrt(
1144 (estimates[:, 0] - true_states[:, 0]) ** 2
1145 + (estimates[:, 2] - true_states[:, 2]) ** 2
1146 + (estimates[:, 4] - true_states[:, 4]) ** 2
1147 )
1148
1149 err_low = compute_errors(est_low)
1150 err_high = compute_errors(est_high)
1151
1152 print("\n3D Position RMSE:")
1153 print(f" Low process noise (q={q_low}): {np.sqrt(np.mean(err_low**2)):.2f} m")
1154 print(f" High process noise (q={q_high}): {np.sqrt(np.mean(err_high**2)):.2f} m")
1155
1156 # Error during maneuver phases
1157 print("\nRMSE by phase:")
1158 phases = [
1159 (0, 40, "Straight"),
1160 (40, 80, "Climbing turn"),
1161 (80, 120, "Descending turn"),
1162 ]
1163 for start, end, name in phases:
1164 rmse_low = np.sqrt(np.mean(err_low[start:end] ** 2))
1165 rmse_high = np.sqrt(np.mean(err_high[start:end] ** 2))
1166 print(f" {name:18s}: Low Q = {rmse_low:.2f} m, High Q = {rmse_high:.2f} m")
1167
1168 # Plot
1169 if SHOW_PLOTS:
1170 time = np.arange(n_steps)
1171
1172 fig = make_subplots(
1173 rows=1,
1174 cols=3,
1175 specs=[[{"type": "scatter3d"}, {"type": "xy"}, {"type": "xy"}]],
1176 subplot_titles=(
1177 "Maneuvering Target Tracking",
1178 "Error Comparison",
1179 "Altitude Profile",
1180 ),
1181 )
1182
1183 # 3D trajectory
1184 fig.add_trace(
1185 go.Scatter3d(
1186 x=true_states[:, 0],
1187 y=true_states[:, 2],
1188 z=true_states[:, 4],
1189 mode="lines",
1190 name="True",
1191 line=dict(color="black", width=4),
1192 ),
1193 row=1,
1194 col=1,
1195 )
1196 fig.add_trace(
1197 go.Scatter3d(
1198 x=est_low[:, 0],
1199 y=est_low[:, 2],
1200 z=est_low[:, 4],
1201 mode="lines",
1202 name=f"Low Q ({q_low})",
1203 line=dict(color="blue", width=2, dash="dash"),
1204 opacity=0.7,
1205 ),
1206 row=1,
1207 col=1,
1208 )
1209 fig.add_trace(
1210 go.Scatter3d(
1211 x=est_high[:, 0],
1212 y=est_high[:, 2],
1213 z=est_high[:, 4],
1214 mode="lines",
1215 name=f"High Q ({q_high})",
1216 line=dict(color="red", width=2, dash="dash"),
1217 opacity=0.7,
1218 ),
1219 row=1,
1220 col=1,
1221 )
1222
1223 # Error over time
1224 fig.add_trace(
1225 go.Scatter(
1226 x=time,
1227 y=err_low,
1228 mode="lines",
1229 name=f"Low Q ({q_low})",
1230 line=dict(color="blue", width=2),
1231 opacity=0.7,
1232 ),
1233 row=1,
1234 col=2,
1235 )
1236 fig.add_trace(
1237 go.Scatter(
1238 x=time,
1239 y=err_high,
1240 mode="lines",
1241 name=f"High Q ({q_high})",
1242 line=dict(color="red", width=2),
1243 opacity=0.7,
1244 ),
1245 row=1,
1246 col=2,
1247 )
1248 # Add phase markers using shapes (more compatible with mixed subplot types)
1249 for boundary in [40, 80]:
1250 fig.add_shape(
1251 type="line",
1252 x0=boundary,
1253 x1=boundary,
1254 y0=0,
1255 y1=1,
1256 yref="y2 domain",
1257 xref="x2",
1258 line=dict(dash="dash", color="gray"),
1259 )
1260
1261 # Altitude profile
1262 fig.add_trace(
1263 go.Scatter(
1264 x=time,
1265 y=true_states[:, 4],
1266 mode="lines",
1267 name="True",
1268 line=dict(color="black", width=3),
1269 showlegend=False,
1270 ),
1271 row=1,
1272 col=3,
1273 )
1274 fig.add_trace(
1275 go.Scatter(
1276 x=time,
1277 y=est_low[:, 4],
1278 mode="lines",
1279 name="Low Q",
1280 line=dict(color="blue", width=2, dash="dash"),
1281 opacity=0.7,
1282 showlegend=False,
1283 ),
1284 row=1,
1285 col=3,
1286 )
1287 fig.add_trace(
1288 go.Scatter(
1289 x=time,
1290 y=est_high[:, 4],
1291 mode="lines",
1292 name="High Q",
1293 line=dict(color="red", width=2, dash="dash"),
1294 opacity=0.7,
1295 showlegend=False,
1296 ),
1297 row=1,
1298 col=3,
1299 )
1300 # Add phase markers using shapes (compatible with mixed subplot types)
1301 for boundary in [40, 80]:
1302 fig.add_shape(
1303 type="line",
1304 x0=boundary,
1305 x1=boundary,
1306 y0=0,
1307 y1=1,
1308 yref="y3 domain",
1309 xref="x3",
1310 line=dict(dash="dash", color="gray"),
1311 )
1312
1313 fig.update_layout(
1314 title="Maneuvering Target Tracking",
1315 height=500,
1316 width=1400,
1317 showlegend=True,
1318 )
1319 fig.update_xaxes(title_text="Time step", row=1, col=2)
1320 fig.update_yaxes(title_text="3D Position Error (m)", row=1, col=2)
1321 fig.update_xaxes(title_text="Time step", row=1, col=3)
1322 fig.update_yaxes(title_text="Altitude Z (m)", row=1, col=3)
1323
1324 fig.write_html(
1325 str(OUTPUT_DIR / "tracking_3d_maneuver.html"),
1326 include_plotlyjs="cdn",
1327 div_id="tracking_3d_maneuver",
1328 )
1329 print("\n [Plot saved to tracking_3d_maneuver.html]")
1330
1331
1332def main():
1333 """Run all demonstrations."""
1334 print("\n" + "#" * 70)
1335 print("# PyTCL 3D Tracking Example")
1336 print("#" * 70)
1337
1338 # Basic 3D tracking
1339 demo_3d_kalman_filter()
1340 demo_3d_rts_smoother()
1341
1342 # Advanced scenarios
1343 demo_spherical_measurements()
1344 demo_multi_sensor_3d()
1345 demo_3d_maneuvering_target()
1346
1347 print("\n" + "=" * 70)
1348 print("Example complete!")
1349 if SHOW_PLOTS:
1350 print("Plots saved: tracking_3d_kalman.html, tracking_3d_smoother.html,")
1351 print(" tracking_3d_radar.html, tracking_3d_multisensor.html,")
1352 print(" tracking_3d_maneuver.html")
1353 print("=" * 70)
1354
1355
1356if __name__ == "__main__":
1357 main()
Running the Example
python examples/tracking_3d.py
See Also
Multi-Target Tracking - Multiple target tracking
Coordinate Systems - Coordinate transformations
Kalman Filter Comparison - Filter variants