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 models: Constant velocity, coordinated turn in 3D
Visualization: Displaying tracks and uncertainty in 3D
Key Concepts
Spherical-to-Cartesian conversion:
sphere2cart()andcart2sphere()Measurement Jacobians: Linearization for EKF updates
3D covariance ellipsoids: Visualizing uncertainty in 3D
Helical trajectories: Constant turn rate with vertical motion
Code Highlights
The example demonstrates:
9-state model: [x, vx, ax, y, vy, ay, z, vz, az]
Range-azimuth-elevation measurement model
EKF with spherical measurement Jacobian
Plotly 3D visualization with trajectory and uncertainty
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(str(OUTPUT_DIR / "tracking_3d_kalman.html"))
334 print("\n [Plot saved to tracking_3d_kalman.html]")
335
336 return estimates, true_states, measurements
337
338
339def demo_3d_rts_smoother():
340 """Demonstrate RTS smoother for 3D tracking."""
341 print("\n" + "=" * 70)
342 print("3D RTS Smoother Demo")
343 print("=" * 70)
344
345 # Generate 3D trajectory
346 n_steps = 100
347 true_states, measurements, F, Q, H, R = generate_3d_cv_trajectory(n_steps=n_steps)
348
349 # Initial state
350 x0 = np.array([0, 0, 0, 0, 1000, 0])
351 P0 = np.diag([100, 50, 100, 50, 100, 50])
352
353 # Run RTS smoother
354 result = rts_smoother(x0, P0, measurements, F, Q, H, R)
355
356 # Compute errors for filter vs smoother
357 filter_pos_errors = []
358 smooth_pos_errors = []
359
360 for k in range(n_steps):
361 true = true_states[k, [0, 2, 4]] # x, y, z
362
363 filt_pos = result.x_filt[k][[0, 2, 4]]
364 smooth_pos = result.x_smooth[k][[0, 2, 4]]
365
366 filter_pos_errors.append(np.linalg.norm(filt_pos - true))
367 smooth_pos_errors.append(np.linalg.norm(smooth_pos - true))
368
369 print("\n3D Position RMSE comparison:")
370 print(f" Filter: {np.sqrt(np.mean(np.array(filter_pos_errors)**2)):.2f} m")
371 print(f" Smoother: {np.sqrt(np.mean(np.array(smooth_pos_errors)**2)):.2f} m")
372
373 improvement = (1 - np.mean(smooth_pos_errors) / np.mean(filter_pos_errors)) * 100
374 print(f" Improvement: {improvement:.1f}%")
375
376 # Velocity comparison
377 filter_vel_errors = []
378 smooth_vel_errors = []
379
380 for k in range(n_steps):
381 true = true_states[k, [1, 3, 5]] # vx, vy, vz
382
383 filt_vel = result.x_filt[k][[1, 3, 5]]
384 smooth_vel = result.x_smooth[k][[1, 3, 5]]
385
386 filter_vel_errors.append(np.linalg.norm(filt_vel - true))
387 smooth_vel_errors.append(np.linalg.norm(smooth_vel - true))
388
389 print("\n3D Velocity RMSE comparison:")
390 print(f" Filter: {np.sqrt(np.mean(np.array(filter_vel_errors)**2)):.2f} m/s")
391 print(f" Smoother: {np.sqrt(np.mean(np.array(smooth_vel_errors)**2)):.2f} m/s")
392
393 vel_improvement = (
394 1 - np.mean(smooth_vel_errors) / np.mean(filter_vel_errors)
395 ) * 100
396 print(f" Improvement: {vel_improvement:.1f}%")
397
398 # Plot comparison
399 if SHOW_PLOTS:
400 smooth_states = np.array(result.x_smooth)
401 time = np.arange(n_steps)
402
403 fig = make_subplots(
404 rows=1,
405 cols=3,
406 specs=[[{"type": "scatter3d"}, {"type": "xy"}, {"type": "xy"}]],
407 subplot_titles=(
408 "3D Smoothed Trajectory",
409 "Filter vs Smoother Error",
410 "Uncertainty Comparison",
411 ),
412 )
413
414 # 3D trajectory comparison
415 fig.add_trace(
416 go.Scatter3d(
417 x=true_states[:, 0],
418 y=true_states[:, 2],
419 z=true_states[:, 4],
420 mode="lines",
421 name="True",
422 line=dict(color="blue", width=4),
423 ),
424 row=1,
425 col=1,
426 )
427 fig.add_trace(
428 go.Scatter3d(
429 x=smooth_states[:, 0],
430 y=smooth_states[:, 2],
431 z=smooth_states[:, 4],
432 mode="lines",
433 name="Smoothed",
434 line=dict(color="red", width=3, dash="dash"),
435 ),
436 row=1,
437 col=1,
438 )
439
440 # Position error comparison
441 fig.add_trace(
442 go.Scatter(
443 x=time,
444 y=filter_pos_errors,
445 mode="lines",
446 name="Filter",
447 line=dict(color="blue", width=2),
448 opacity=0.7,
449 ),
450 row=1,
451 col=2,
452 )
453 fig.add_trace(
454 go.Scatter(
455 x=time,
456 y=smooth_pos_errors,
457 mode="lines",
458 name="Smoother",
459 line=dict(color="red", width=2),
460 opacity=0.7,
461 ),
462 row=1,
463 col=2,
464 )
465
466 # Uncertainty comparison (trace of P)
467 filter_trace = [np.trace(result.P_filt[k]) for k in range(n_steps)]
468 smooth_trace = [np.trace(result.P_smooth[k]) for k in range(n_steps)]
469 fig.add_trace(
470 go.Scatter(
471 x=time,
472 y=filter_trace,
473 mode="lines",
474 name="Filter",
475 line=dict(color="blue", width=2),
476 showlegend=False,
477 ),
478 row=1,
479 col=3,
480 )
481 fig.add_trace(
482 go.Scatter(
483 x=time,
484 y=smooth_trace,
485 mode="lines",
486 name="Smoother",
487 line=dict(color="red", width=2),
488 showlegend=False,
489 ),
490 row=1,
491 col=3,
492 )
493
494 fig.update_layout(
495 title="RTS Smoother Comparison",
496 height=500,
497 width=1400,
498 showlegend=True,
499 )
500 fig.update_xaxes(title_text="Time step", row=1, col=2)
501 fig.update_yaxes(title_text="3D Position Error (m)", row=1, col=2)
502 fig.update_xaxes(title_text="Time step", row=1, col=3)
503 fig.update_yaxes(title_text="Covariance Trace", row=1, col=3)
504
505 fig.write_html(str(OUTPUT_DIR / "tracking_3d_smoother.html"))
506 print("\n [Plot saved to tracking_3d_smoother.html]")
507
508
509def demo_spherical_measurements():
510 """Demonstrate 3D tracking with spherical (radar) measurements."""
511 print("\n" + "=" * 70)
512 print("Spherical Measurements (Radar) Demo")
513 print("=" * 70)
514
515 np.random.seed(42)
516
517 # Generate true 3D trajectory (aircraft flight path)
518 n_steps = 80
519 dt = 1.0
520
521 # State: [x, vx, y, vy, z, vz]
522 F = np.array(
523 [
524 [1, dt, 0, 0, 0, 0],
525 [0, 1, 0, 0, 0, 0],
526 [0, 0, 1, dt, 0, 0],
527 [0, 0, 0, 1, 0, 0],
528 [0, 0, 0, 0, 1, dt],
529 [0, 0, 0, 0, 0, 1],
530 ]
531 )
532
533 q = 0.5
534 Q_1d = np.array([[dt**3 / 3, dt**2 / 2], [dt**2 / 2, dt]]) * q
535 Q = np.zeros((6, 6))
536 Q[0:2, 0:2] = Q_1d
537 Q[2:4, 2:4] = Q_1d
538 Q[4:6, 4:6] = Q_1d
539
540 # True trajectory - aircraft at 10km altitude, approaching
541 true_states = np.zeros((n_steps, 6))
542 true_states[0] = [50000, -200, 30000, -100, 10000, 0] # 50km away, approaching
543
544 for k in range(1, n_steps):
545 process_noise = np.random.multivariate_normal(np.zeros(6), Q * 0.1)
546 true_states[k] = F @ true_states[k - 1] + process_noise
547
548 print("\nScenario: Aircraft tracking with radar")
549 print(
550 f" Initial position: ({true_states[0, 0]/1000:.1f}, "
551 f"{true_states[0, 2]/1000:.1f}, {true_states[0, 4]/1000:.1f}) km"
552 )
553 print(
554 f" Final position: ({true_states[-1, 0]/1000:.1f}, "
555 f"{true_states[-1, 2]/1000:.1f}, {true_states[-1, 4]/1000:.1f}) km"
556 )
557
558 # Radar measurement function: Cartesian -> (range, azimuth, elevation)
559 def cartesian_to_spherical(x, y, z):
560 r = np.sqrt(x**2 + y**2 + z**2)
561 az = np.arctan2(y, x)
562 el = np.arctan2(z, np.sqrt(x**2 + y**2))
563 return np.array([r, az, el])
564
565 def spherical_to_cartesian(r, az, el):
566 x = r * np.cos(el) * np.cos(az)
567 y = r * np.cos(el) * np.sin(az)
568 z = r * np.sin(el)
569 return np.array([x, y, z])
570
571 # Measurement noise (typical radar)
572 sigma_r = 50.0 # 50m range noise
573 sigma_az = np.radians(0.5) # 0.5 degree azimuth noise
574 sigma_el = np.radians(0.5) # 0.5 degree elevation noise
575 R_spherical = np.diag([sigma_r**2, sigma_az**2, sigma_el**2])
576
577 # Generate spherical measurements
578 spherical_measurements = []
579 for k in range(n_steps):
580 x, y, z = true_states[k, 0], true_states[k, 2], true_states[k, 4]
581 z_true = cartesian_to_spherical(x, y, z)
582 noise = np.array(
583 [
584 np.random.randn() * sigma_r,
585 np.random.randn() * sigma_az,
586 np.random.randn() * sigma_el,
587 ]
588 )
589 spherical_measurements.append(z_true + noise)
590
591 print(f"\nMeasurement noise:")
592 print(f" Range: {sigma_r} m")
593 print(f" Azimuth: {np.degrees(sigma_az):.2f} deg")
594 print(f" Elevation: {np.degrees(sigma_el):.2f} deg")
595
596 # Convert measurements to Cartesian for simple Kalman filter
597 cartesian_measurements = []
598 for z_sph in spherical_measurements:
599 r, az, el = z_sph
600 cart = spherical_to_cartesian(r, az, el)
601 cartesian_measurements.append(cart)
602
603 # Measurement matrix for Cartesian measurements
604 H = np.array(
605 [
606 [1, 0, 0, 0, 0, 0],
607 [0, 0, 1, 0, 0, 0],
608 [0, 0, 0, 0, 1, 0],
609 ]
610 )
611
612 # Approximate Cartesian measurement noise at mid-range
613 avg_range = np.mean([z[0] for z in spherical_measurements])
614 R_cart = np.diag(
615 [
616 sigma_r**2 + (avg_range * sigma_az) ** 2,
617 sigma_r**2 + (avg_range * sigma_az) ** 2,
618 sigma_r**2 + (avg_range * sigma_el) ** 2,
619 ]
620 )
621
622 # Run Kalman filter
623 x = np.array([50000, 0, 30000, 0, 10000, 0]) # Initial guess
624 P = np.diag([10000, 500, 10000, 500, 5000, 100])
625
626 estimates = []
627 for k in range(n_steps):
628 # Predict
629 x = F @ x
630 P = F @ P @ F.T + Q
631
632 # Update with Cartesian measurement
633 z = cartesian_measurements[k]
634 y = z - H @ x
635 S = H @ P @ H.T + R_cart
636 K = P @ H.T @ np.linalg.inv(S)
637 x = x + K @ y
638 P = (np.eye(6) - K @ H) @ P
639
640 estimates.append(x.copy())
641
642 estimates = np.array(estimates)
643
644 # Compute 3D position errors
645 pos_errors = np.sqrt(
646 (estimates[:, 0] - true_states[:, 0]) ** 2
647 + (estimates[:, 2] - true_states[:, 2]) ** 2
648 + (estimates[:, 4] - true_states[:, 4]) ** 2
649 )
650
651 print(f"\n3D Position RMSE: {np.sqrt(np.mean(pos_errors**2)):.1f} m")
652
653 # Show range over time
654 ranges = [np.sqrt(s[0] ** 2 + s[2] ** 2 + s[4] ** 2) for s in true_states]
655 print(f"\nRange: {ranges[0]/1000:.1f} km -> {ranges[-1]/1000:.1f} km")
656
657 # Plot
658 if SHOW_PLOTS:
659 sph_arr = np.array(spherical_measurements)
660 ranges_km = np.array(ranges) / 1000
661
662 fig = make_subplots(
663 rows=1,
664 cols=3,
665 specs=[[{"type": "scatter3d"}, {"type": "xy"}, {"type": "xy"}]],
666 subplot_titles=(
667 "Radar Tracking (3D)",
668 "Error vs Range",
669 "Radar Measurements (Spherical)",
670 ),
671 )
672
673 # 3D trajectory
674 fig.add_trace(
675 go.Scatter3d(
676 x=true_states[:, 0] / 1000,
677 y=true_states[:, 2] / 1000,
678 z=true_states[:, 4] / 1000,
679 mode="lines",
680 name="True",
681 line=dict(color="blue", width=4),
682 ),
683 row=1,
684 col=1,
685 )
686 fig.add_trace(
687 go.Scatter3d(
688 x=estimates[:, 0] / 1000,
689 y=estimates[:, 2] / 1000,
690 z=estimates[:, 4] / 1000,
691 mode="lines",
692 name="Estimate",
693 line=dict(color="red", width=3, dash="dash"),
694 ),
695 row=1,
696 col=1,
697 )
698 fig.add_trace(
699 go.Scatter3d(
700 x=[0],
701 y=[0],
702 z=[0],
703 mode="markers",
704 name="Radar",
705 marker=dict(color="green", size=8, symbol="diamond"),
706 ),
707 row=1,
708 col=1,
709 )
710
711 # Position error vs range
712 fig.add_trace(
713 go.Scatter(
714 x=ranges_km,
715 y=pos_errors,
716 mode="markers",
717 name="Error",
718 marker=dict(color="blue", size=6, opacity=0.6),
719 ),
720 row=1,
721 col=2,
722 )
723
724 # Spherical measurements (Az vs El, colored by range)
725 fig.add_trace(
726 go.Scatter(
727 x=np.degrees(sph_arr[:, 1]),
728 y=np.degrees(sph_arr[:, 2]),
729 mode="markers",
730 name="Measurements",
731 marker=dict(
732 color=sph_arr[:, 0] / 1000,
733 colorscale="Viridis",
734 size=8,
735 colorbar=dict(title="Range (km)", x=1.02),
736 ),
737 ),
738 row=1,
739 col=3,
740 )
741
742 fig.update_layout(
743 title="Spherical (Radar) Measurements Tracking",
744 height=500,
745 width=1400,
746 showlegend=True,
747 )
748 fig.update_xaxes(title_text="Range (km)", row=1, col=2)
749 fig.update_yaxes(title_text="3D Position Error (m)", row=1, col=2)
750 fig.update_xaxes(title_text="Azimuth (deg)", row=1, col=3)
751 fig.update_yaxes(title_text="Elevation (deg)", row=1, col=3)
752
753 fig.write_html(str(OUTPUT_DIR / "tracking_3d_radar.html"))
754 print("\n [Plot saved to tracking_3d_radar.html]")
755
756
757def demo_multi_sensor_3d():
758 """Demonstrate 3D tracking with multiple sensors."""
759 print("\n" + "=" * 70)
760 print("Multi-Sensor 3D Fusion Demo")
761 print("=" * 70)
762
763 np.random.seed(42)
764
765 # Generate 3D trajectory
766 n_steps = 60
767 dt = 1.0
768
769 F = np.array(
770 [
771 [1, dt, 0, 0, 0, 0],
772 [0, 1, 0, 0, 0, 0],
773 [0, 0, 1, dt, 0, 0],
774 [0, 0, 0, 1, 0, 0],
775 [0, 0, 0, 0, 1, dt],
776 [0, 0, 0, 0, 0, 1],
777 ]
778 )
779
780 q = 0.2
781 Q_1d = np.array([[dt**3 / 3, dt**2 / 2], [dt**2 / 2, dt]]) * q
782 Q = np.zeros((6, 6))
783 Q[0:2, 0:2] = Q_1d
784 Q[2:4, 2:4] = Q_1d
785 Q[4:6, 4:6] = Q_1d
786
787 # True trajectory
788 true_states = np.zeros((n_steps, 6))
789 true_states[0] = [0, 10, 0, 5, 100, 2]
790
791 for k in range(1, n_steps):
792 process_noise = np.random.multivariate_normal(np.zeros(6), Q * 0.1)
793 true_states[k] = F @ true_states[k - 1] + process_noise
794
795 # Define sensors with different noise characteristics
796 sensors = [
797 {"name": "GPS", "noise_std": [3.0, 3.0, 5.0]}, # x, y, z noise in meters
798 {"name": "Radar", "noise_std": [5.0, 5.0, 8.0]},
799 {"name": "Lidar", "noise_std": [0.5, 0.5, 0.8]}, # Most accurate
800 ]
801
802 print("\nSensors:")
803 for s in sensors:
804 print(f" {s['name']}: noise std = {s['noise_std']}")
805
806 # Generate measurements from each sensor
807 sensor_measurements = {s["name"]: [] for s in sensors}
808
809 for k in range(n_steps):
810 true_pos = true_states[k, [0, 2, 4]] # x, y, z
811 for sensor in sensors:
812 noise = np.array(
813 [
814 np.random.randn() * sensor["noise_std"][0],
815 np.random.randn() * sensor["noise_std"][1],
816 np.random.randn() * sensor["noise_std"][2],
817 ]
818 )
819 sensor_measurements[sensor["name"]].append(true_pos + noise)
820
821 H = np.array(
822 [
823 [1, 0, 0, 0, 0, 0],
824 [0, 0, 1, 0, 0, 0],
825 [0, 0, 0, 0, 1, 0],
826 ]
827 )
828
829 # Track using individual sensors and fused
830 results = {}
831
832 for sensor in sensors:
833 R = np.diag([s**2 for s in sensor["noise_std"]])
834 x = np.array([0, 0, 0, 0, 100, 0])
835 P = np.diag([100, 50, 100, 50, 100, 50])
836
837 estimates = []
838 for k in range(n_steps):
839 x = F @ x
840 P = F @ P @ F.T + Q
841
842 z = sensor_measurements[sensor["name"]][k]
843 y = z - H @ x
844 S = H @ P @ H.T + R
845 K = P @ H.T @ np.linalg.inv(S)
846 x = x + K @ y
847 P = (np.eye(6) - K @ H) @ P
848
849 estimates.append(x.copy())
850
851 results[sensor["name"]] = np.array(estimates)
852
853 # Fused tracking (combine all sensor measurements)
854 R_fused = np.zeros((3, 3))
855 for sensor in sensors:
856 R_inv = np.diag([1 / s**2 for s in sensor["noise_std"]])
857 R_fused += R_inv
858 R_fused = np.linalg.inv(R_fused)
859
860 x = np.array([0, 0, 0, 0, 100, 0])
861 P = np.diag([100, 50, 100, 50, 100, 50])
862
863 fused_estimates = []
864 for k in range(n_steps):
865 x = F @ x
866 P = F @ P @ F.T + Q
867
868 # Fuse measurements using information form
869 z_fused = np.zeros(3)
870 for sensor in sensors:
871 R_inv = np.diag([1 / s**2 for s in sensor["noise_std"]])
872 z_fused += R_inv @ sensor_measurements[sensor["name"]][k]
873 z_fused = R_fused @ z_fused
874
875 y = z_fused - H @ x
876 S = H @ P @ H.T + R_fused
877 K = P @ H.T @ np.linalg.inv(S)
878 x = x + K @ y
879 P = (np.eye(6) - K @ H) @ P
880
881 fused_estimates.append(x.copy())
882
883 results["Fused"] = np.array(fused_estimates)
884
885 # Compute RMSE for each
886 print("\n3D Position RMSE:")
887 print("-" * 40)
888 for name, estimates in results.items():
889 errors = np.sqrt(
890 (estimates[:, 0] - true_states[:, 0]) ** 2
891 + (estimates[:, 2] - true_states[:, 2]) ** 2
892 + (estimates[:, 4] - true_states[:, 4]) ** 2
893 )
894 rmse = np.sqrt(np.mean(errors**2))
895 print(f" {name:10s}: {rmse:.3f} m")
896
897 # Plot
898 if SHOW_PLOTS:
899 time = np.arange(n_steps)
900 colors = {"GPS": "blue", "Radar": "orange", "Lidar": "green", "Fused": "red"}
901
902 fig = make_subplots(
903 rows=1,
904 cols=3,
905 specs=[[{"type": "scatter3d"}, {"type": "xy"}, {"type": "xy"}]],
906 subplot_titles=(
907 "Multi-Sensor 3D Tracking",
908 "Position Error Comparison",
909 "XZ Projection (Side View)",
910 ),
911 )
912
913 # 3D trajectory
914 fig.add_trace(
915 go.Scatter3d(
916 x=true_states[:, 0],
917 y=true_states[:, 2],
918 z=true_states[:, 4],
919 mode="lines",
920 name="True",
921 line=dict(color="black", width=4),
922 ),
923 row=1,
924 col=1,
925 )
926 for name in ["GPS", "Fused"]:
927 est = results[name]
928 fig.add_trace(
929 go.Scatter3d(
930 x=est[:, 0],
931 y=est[:, 2],
932 z=est[:, 4],
933 mode="lines",
934 name=name,
935 line=dict(
936 color=colors[name],
937 width=3,
938 dash="dash" if name != "Fused" else "solid",
939 ),
940 ),
941 row=1,
942 col=1,
943 )
944
945 # Error comparison
946 for name, estimates in results.items():
947 errors = np.sqrt(
948 (estimates[:, 0] - true_states[:, 0]) ** 2
949 + (estimates[:, 2] - true_states[:, 2]) ** 2
950 + (estimates[:, 4] - true_states[:, 4]) ** 2
951 )
952 fig.add_trace(
953 go.Scatter(
954 x=time,
955 y=errors,
956 mode="lines",
957 name=name,
958 line=dict(color=colors[name], width=2),
959 opacity=0.8,
960 ),
961 row=1,
962 col=2,
963 )
964
965 # XZ projection (side view)
966 fig.add_trace(
967 go.Scatter(
968 x=true_states[:, 0],
969 y=true_states[:, 4],
970 mode="lines",
971 name="True",
972 line=dict(color="black", width=3),
973 showlegend=False,
974 ),
975 row=1,
976 col=3,
977 )
978 fig.add_trace(
979 go.Scatter(
980 x=results["Fused"][:, 0],
981 y=results["Fused"][:, 4],
982 mode="lines",
983 name="Fused",
984 line=dict(color="red", width=2),
985 showlegend=False,
986 ),
987 row=1,
988 col=3,
989 )
990
991 fig.update_layout(
992 title="Multi-Sensor 3D Fusion",
993 height=500,
994 width=1400,
995 showlegend=True,
996 )
997 fig.update_xaxes(title_text="Time step", row=1, col=2)
998 fig.update_yaxes(title_text="3D Position Error (m)", row=1, col=2)
999 fig.update_xaxes(title_text="X (m)", row=1, col=3)
1000 fig.update_yaxes(title_text="Z (m)", row=1, col=3)
1001
1002 fig.write_html(str(OUTPUT_DIR / "tracking_3d_multisensor.html"))
1003 print("\n [Plot saved to tracking_3d_multisensor.html]")
1004
1005
1006def demo_3d_maneuvering_target():
1007 """Demonstrate tracking a maneuvering target in 3D."""
1008 print("\n" + "=" * 70)
1009 print("Maneuvering Target Demo")
1010 print("=" * 70)
1011
1012 np.random.seed(42)
1013
1014 n_steps = 120
1015 dt = 1.0
1016
1017 # Generate maneuvering trajectory (includes turns and altitude changes)
1018 true_states = np.zeros((n_steps, 6))
1019 true_states[0] = [0, 50, 0, 0, 1000, 0]
1020
1021 print("\nScenario: Maneuvering aircraft")
1022 print(" Phase 1 (t=0-40): Straight flight")
1023 print(" Phase 2 (t=40-80): Climbing turn")
1024 print(" Phase 3 (t=80-120): Descending turn")
1025
1026 for k in range(1, n_steps):
1027 x, vx, y, vy, z, vz = true_states[k - 1]
1028
1029 if k < 40:
1030 # Straight flight
1031 vx_new, vy_new, vz_new = 50, 0, 0
1032 elif k < 80:
1033 # Climbing turn (increase vy, increase vz)
1034 omega = 0.02 # Turn rate
1035 vx_new = 50 * np.cos(omega * (k - 40))
1036 vy_new = 50 * np.sin(omega * (k - 40))
1037 vz_new = 5 # Climbing
1038 else:
1039 # Descending turn
1040 omega = -0.03
1041 vx_new = 40 * np.cos(omega * (k - 80))
1042 vy_new = 40 * np.sin(omega * (k - 80)) + 30
1043 vz_new = -3 # Descending
1044
1045 # Add process noise
1046 noise = np.random.randn(6) * np.array([1, 0.5, 1, 0.5, 1, 0.5])
1047
1048 true_states[k] = [
1049 x + vx * dt + noise[0],
1050 vx_new + noise[1],
1051 y + vy * dt + noise[2],
1052 vy_new + noise[3],
1053 z + vz * dt + noise[4],
1054 vz_new + noise[5],
1055 ]
1056
1057 # Generate noisy measurements
1058 R = np.diag([4.0, 4.0, 9.0]) # x, y, z measurement noise
1059 H = np.array(
1060 [
1061 [1, 0, 0, 0, 0, 0],
1062 [0, 0, 1, 0, 0, 0],
1063 [0, 0, 0, 0, 1, 0],
1064 ]
1065 )
1066
1067 measurements = []
1068 for k in range(n_steps):
1069 true_pos = true_states[k, [0, 2, 4]]
1070 noise = np.random.multivariate_normal(np.zeros(3), R)
1071 measurements.append(true_pos + noise)
1072
1073 # Constant velocity model (will struggle with maneuvers)
1074 F_cv = np.array(
1075 [
1076 [1, dt, 0, 0, 0, 0],
1077 [0, 1, 0, 0, 0, 0],
1078 [0, 0, 1, dt, 0, 0],
1079 [0, 0, 0, 1, 0, 0],
1080 [0, 0, 0, 0, 1, dt],
1081 [0, 0, 0, 0, 0, 1],
1082 ]
1083 )
1084
1085 # Low process noise (assumes constant velocity)
1086 q_low = 0.1
1087 Q_low = np.zeros((6, 6))
1088 for i in [0, 2, 4]:
1089 Q_low[i : i + 2, i : i + 2] = (
1090 np.array([[dt**3 / 3, dt**2 / 2], [dt**2 / 2, dt]]) * q_low
1091 )
1092
1093 # High process noise (handles maneuvers better)
1094 q_high = 5.0
1095 Q_high = np.zeros((6, 6))
1096 for i in [0, 2, 4]:
1097 Q_high[i : i + 2, i : i + 2] = (
1098 np.array([[dt**3 / 3, dt**2 / 2], [dt**2 / 2, dt]]) * q_high
1099 )
1100
1101 # Track with both process noise levels
1102 def run_filter(Q):
1103 x = np.array([0, 50, 0, 0, 1000, 0])
1104 P = np.diag([100, 50, 100, 50, 100, 50])
1105 estimates = []
1106
1107 for k in range(n_steps):
1108 x = F_cv @ x
1109 P = F_cv @ P @ F_cv.T + Q
1110
1111 z = measurements[k]
1112 y = z - H @ x
1113 S = H @ P @ H.T + R
1114 K = P @ H.T @ np.linalg.inv(S)
1115 x = x + K @ y
1116 P = (np.eye(6) - K @ H) @ P
1117
1118 estimates.append(x.copy())
1119
1120 return np.array(estimates)
1121
1122 est_low = run_filter(Q_low)
1123 est_high = run_filter(Q_high)
1124
1125 # Compute errors
1126 def compute_errors(estimates):
1127 return np.sqrt(
1128 (estimates[:, 0] - true_states[:, 0]) ** 2
1129 + (estimates[:, 2] - true_states[:, 2]) ** 2
1130 + (estimates[:, 4] - true_states[:, 4]) ** 2
1131 )
1132
1133 err_low = compute_errors(est_low)
1134 err_high = compute_errors(est_high)
1135
1136 print("\n3D Position RMSE:")
1137 print(f" Low process noise (q={q_low}): {np.sqrt(np.mean(err_low**2)):.2f} m")
1138 print(f" High process noise (q={q_high}): {np.sqrt(np.mean(err_high**2)):.2f} m")
1139
1140 # Error during maneuver phases
1141 print("\nRMSE by phase:")
1142 phases = [
1143 (0, 40, "Straight"),
1144 (40, 80, "Climbing turn"),
1145 (80, 120, "Descending turn"),
1146 ]
1147 for start, end, name in phases:
1148 rmse_low = np.sqrt(np.mean(err_low[start:end] ** 2))
1149 rmse_high = np.sqrt(np.mean(err_high[start:end] ** 2))
1150 print(f" {name:18s}: Low Q = {rmse_low:.2f} m, High Q = {rmse_high:.2f} m")
1151
1152 # Plot
1153 if SHOW_PLOTS:
1154 time = np.arange(n_steps)
1155
1156 fig = make_subplots(
1157 rows=1,
1158 cols=3,
1159 specs=[[{"type": "scatter3d"}, {"type": "xy"}, {"type": "xy"}]],
1160 subplot_titles=(
1161 "Maneuvering Target Tracking",
1162 "Error Comparison",
1163 "Altitude Profile",
1164 ),
1165 )
1166
1167 # 3D trajectory
1168 fig.add_trace(
1169 go.Scatter3d(
1170 x=true_states[:, 0],
1171 y=true_states[:, 2],
1172 z=true_states[:, 4],
1173 mode="lines",
1174 name="True",
1175 line=dict(color="black", width=4),
1176 ),
1177 row=1,
1178 col=1,
1179 )
1180 fig.add_trace(
1181 go.Scatter3d(
1182 x=est_low[:, 0],
1183 y=est_low[:, 2],
1184 z=est_low[:, 4],
1185 mode="lines",
1186 name=f"Low Q ({q_low})",
1187 line=dict(color="blue", width=2, dash="dash"),
1188 opacity=0.7,
1189 ),
1190 row=1,
1191 col=1,
1192 )
1193 fig.add_trace(
1194 go.Scatter3d(
1195 x=est_high[:, 0],
1196 y=est_high[:, 2],
1197 z=est_high[:, 4],
1198 mode="lines",
1199 name=f"High Q ({q_high})",
1200 line=dict(color="red", width=2, dash="dash"),
1201 opacity=0.7,
1202 ),
1203 row=1,
1204 col=1,
1205 )
1206
1207 # Error over time
1208 fig.add_trace(
1209 go.Scatter(
1210 x=time,
1211 y=err_low,
1212 mode="lines",
1213 name=f"Low Q ({q_low})",
1214 line=dict(color="blue", width=2),
1215 opacity=0.7,
1216 ),
1217 row=1,
1218 col=2,
1219 )
1220 fig.add_trace(
1221 go.Scatter(
1222 x=time,
1223 y=err_high,
1224 mode="lines",
1225 name=f"High Q ({q_high})",
1226 line=dict(color="red", width=2),
1227 opacity=0.7,
1228 ),
1229 row=1,
1230 col=2,
1231 )
1232 # Add phase markers using shapes (more compatible with mixed subplot types)
1233 for boundary in [40, 80]:
1234 fig.add_shape(
1235 type="line",
1236 x0=boundary,
1237 x1=boundary,
1238 y0=0,
1239 y1=1,
1240 yref="y2 domain",
1241 xref="x2",
1242 line=dict(dash="dash", color="gray"),
1243 )
1244
1245 # Altitude profile
1246 fig.add_trace(
1247 go.Scatter(
1248 x=time,
1249 y=true_states[:, 4],
1250 mode="lines",
1251 name="True",
1252 line=dict(color="black", width=3),
1253 showlegend=False,
1254 ),
1255 row=1,
1256 col=3,
1257 )
1258 fig.add_trace(
1259 go.Scatter(
1260 x=time,
1261 y=est_low[:, 4],
1262 mode="lines",
1263 name="Low Q",
1264 line=dict(color="blue", width=2, dash="dash"),
1265 opacity=0.7,
1266 showlegend=False,
1267 ),
1268 row=1,
1269 col=3,
1270 )
1271 fig.add_trace(
1272 go.Scatter(
1273 x=time,
1274 y=est_high[:, 4],
1275 mode="lines",
1276 name="High Q",
1277 line=dict(color="red", width=2, dash="dash"),
1278 opacity=0.7,
1279 showlegend=False,
1280 ),
1281 row=1,
1282 col=3,
1283 )
1284 # Add phase markers using shapes (compatible with mixed subplot types)
1285 for boundary in [40, 80]:
1286 fig.add_shape(
1287 type="line",
1288 x0=boundary,
1289 x1=boundary,
1290 y0=0,
1291 y1=1,
1292 yref="y3 domain",
1293 xref="x3",
1294 line=dict(dash="dash", color="gray"),
1295 )
1296
1297 fig.update_layout(
1298 title="Maneuvering Target Tracking",
1299 height=500,
1300 width=1400,
1301 showlegend=True,
1302 )
1303 fig.update_xaxes(title_text="Time step", row=1, col=2)
1304 fig.update_yaxes(title_text="3D Position Error (m)", row=1, col=2)
1305 fig.update_xaxes(title_text="Time step", row=1, col=3)
1306 fig.update_yaxes(title_text="Altitude Z (m)", row=1, col=3)
1307
1308 fig.write_html(str(OUTPUT_DIR / "tracking_3d_maneuver.html"))
1309 print("\n [Plot saved to tracking_3d_maneuver.html]")
1310
1311
1312def main():
1313 """Run all demonstrations."""
1314 print("\n" + "#" * 70)
1315 print("# PyTCL 3D Tracking Example")
1316 print("#" * 70)
1317
1318 # Basic 3D tracking
1319 demo_3d_kalman_filter()
1320 demo_3d_rts_smoother()
1321
1322 # Advanced scenarios
1323 demo_spherical_measurements()
1324 demo_multi_sensor_3d()
1325 demo_3d_maneuvering_target()
1326
1327 print("\n" + "=" * 70)
1328 print("Example complete!")
1329 if SHOW_PLOTS:
1330 print("Plots saved: tracking_3d_kalman.html, tracking_3d_smoother.html,")
1331 print(" tracking_3d_radar.html, tracking_3d_multisensor.html,")
1332 print(" tracking_3d_maneuver.html")
1333 print("=" * 70)
1334
1335
1336if __name__ == "__main__":
1337 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