Filter Uncertainty Visualization
This example visualizes filter covariance ellipses and uncertainty propagation.
Overview
Understanding and visualizing filter uncertainty is crucial for:
Tuning filter parameters - Ensuring appropriate uncertainty levels
Detecting filter divergence - Identifying when estimates become unreliable
Validating consistency - Checking that actual errors match predicted uncertainty
Key Concepts
Covariance ellipses: 2D/3D visualization of multivariate Gaussian uncertainty
Uncertainty propagation: How uncertainty grows during prediction steps
Measurement updates: How measurements reduce uncertainty
Sigma contours: 1-sigma, 2-sigma, 3-sigma probability regions
Code Highlights
The example demonstrates:
Plotting covariance ellipses from filter covariance matrices
Animating uncertainty evolution over time
Comparing predicted vs actual estimation errors
Visualizing measurement update effects
Source Code
1"""
2Filter Uncertainty and Covariance Visualization.
3
4This example demonstrates:
51. Plotting covariance ellipses for Kalman filter estimates
62. Visualizing how uncertainty evolves over time
73. Comparing filter predictions with ground truth
84. Animated tracking with uncertainty bounds
9
10Run with: python examples/filter_uncertainty_visualization.py
11"""
12
13import sys
14from pathlib import Path
15
16sys.path.insert(0, str(Path(__file__).parent.parent))
17
18# Output directory for generated plots
19OUTPUT_DIR = Path(__file__).parent.parent / "docs" / "_static" / "images" / "examples"
20OUTPUT_DIR.mkdir(parents=True, exist_ok=True)
21
22import os
23
24import numpy as np # noqa: E402
25import plotly.graph_objects as go # noqa: E402
26from plotly.subplots import make_subplots # noqa: E402
27
28from pytcl.dynamic_estimation.kalman import ( # noqa: E402
29 kf_predict,
30 kf_update,
31)
32from pytcl.dynamic_models import ( # noqa: E402
33 f_constant_velocity,
34 q_constant_velocity,
35)
36
37SHOW_PLOTS = os.environ.get("PYTCL_SHOW_PLOTS", "1") != "0"
38
39
40def covariance_ellipse(
41 mean: np.ndarray,
42 cov: np.ndarray,
43 n_std: float = 2.0,
44 n_points: int = 100,
45) -> tuple:
46 """
47 Generate points for a 2D covariance ellipse.
48
49 Parameters
50 ----------
51 mean : ndarray
52 Center of the ellipse (x, y).
53 cov : ndarray
54 2x2 covariance matrix.
55 n_std : float
56 Number of standard deviations for ellipse size.
57 n_points : int
58 Number of points to generate.
59
60 Returns
61 -------
62 x, y : ndarray
63 Ellipse coordinates.
64 """
65 # Eigendecomposition
66 eigenvalues, eigenvectors = np.linalg.eigh(cov)
67
68 # Sort by eigenvalue (largest first)
69 order = eigenvalues.argsort()[::-1]
70 eigenvalues = eigenvalues[order]
71 eigenvectors = eigenvectors[:, order]
72
73 # Compute angle
74 angle = np.arctan2(eigenvectors[1, 0], eigenvectors[0, 0])
75
76 # Generate ellipse points
77 t = np.linspace(0, 2 * np.pi, n_points)
78 a = n_std * np.sqrt(eigenvalues[0])
79 b = n_std * np.sqrt(eigenvalues[1])
80
81 # Ellipse in standard position
82 x_std = a * np.cos(t)
83 y_std = b * np.sin(t)
84
85 # Rotate and translate
86 x = mean[0] + x_std * np.cos(angle) - y_std * np.sin(angle)
87 y = mean[1] + x_std * np.sin(angle) + y_std * np.cos(angle)
88
89 return x, y
90
91
92def simulate_tracking_scenario(n_steps: int = 50, dt: float = 1.0) -> dict:
93 """
94 Simulate a target tracking scenario with Kalman filter.
95
96 Returns
97 -------
98 dict
99 Contains true states, measurements, estimates, and covariances.
100 """
101 # True initial state [x, vx, y, vy]
102 x_true = np.array([0.0, 2.0, 0.0, 1.5])
103
104 # Process and measurement noise
105 sigma_a = 0.3 # Acceleration noise
106 sigma_z = 2.0 # Measurement noise
107
108 # State transition and process noise
109 F = f_constant_velocity(T=dt, num_dims=2)
110 Q = q_constant_velocity(T=dt, sigma_a=sigma_a, num_dims=2)
111
112 # Measurement matrix (measure position only)
113 H = np.array([[1, 0, 0, 0], [0, 0, 1, 0]])
114 R = np.eye(2) * sigma_z**2
115
116 # Initial estimate
117 x_est = np.array([0.0, 0.0, 0.0, 0.0])
118 P_est = np.diag([10.0, 5.0, 10.0, 5.0])
119
120 # Storage
121 true_states = [x_true.copy()]
122 measurements = []
123 estimates = [x_est.copy()]
124 covariances = [P_est.copy()]
125 predicted_states = []
126 predicted_covariances = []
127
128 np.random.seed(42)
129
130 for k in range(n_steps):
131 # Propagate true state with some process noise
132 process_noise = np.random.multivariate_normal(np.zeros(4), Q)
133 x_true = F @ x_true + process_noise
134
135 # Generate measurement
136 z = H @ x_true + np.random.multivariate_normal(np.zeros(2), R)
137
138 # Kalman filter predict
139 pred = kf_predict(x_est, P_est, F, Q)
140 predicted_states.append(pred.x.copy())
141 predicted_covariances.append(pred.P.copy())
142
143 # Kalman filter update
144 upd = kf_update(pred.x, pred.P, z, H, R)
145 x_est = upd.x
146 P_est = upd.P
147
148 # Store
149 true_states.append(x_true.copy())
150 measurements.append(z.copy())
151 estimates.append(x_est.copy())
152 covariances.append(P_est.copy())
153
154 return {
155 "true_states": np.array(true_states),
156 "measurements": np.array(measurements),
157 "estimates": np.array(estimates),
158 "covariances": covariances,
159 "predicted_states": np.array(predicted_states),
160 "predicted_covariances": predicted_covariances,
161 "dt": dt,
162 }
163
164
165def plot_tracking_with_ellipses(data: dict) -> go.Figure:
166 """
167 Plot tracking results with covariance ellipses.
168 """
169 fig = go.Figure()
170
171 true_states = data["true_states"]
172 measurements = data["measurements"]
173 estimates = data["estimates"]
174 covariances = data["covariances"]
175
176 # True trajectory
177 fig.add_trace(
178 go.Scatter(
179 x=true_states[:, 0],
180 y=true_states[:, 2],
181 mode="lines",
182 line=dict(color="green", width=3),
183 name="True trajectory",
184 )
185 )
186
187 # Measurements
188 fig.add_trace(
189 go.Scatter(
190 x=measurements[:, 0],
191 y=measurements[:, 1],
192 mode="markers",
193 marker=dict(color="black", size=6, symbol="x"),
194 name="Measurements",
195 )
196 )
197
198 # Estimated trajectory
199 fig.add_trace(
200 go.Scatter(
201 x=estimates[:, 0],
202 y=estimates[:, 2],
203 mode="lines+markers",
204 line=dict(color="blue", width=2),
205 marker=dict(size=4),
206 name="Kalman estimate",
207 )
208 )
209
210 # Covariance ellipses (every 5 steps)
211 for i in range(0, len(estimates), 5):
212 pos_mean = np.array([estimates[i, 0], estimates[i, 2]])
213 pos_cov = np.array(
214 [
215 [covariances[i][0, 0], covariances[i][0, 2]],
216 [covariances[i][2, 0], covariances[i][2, 2]],
217 ]
218 )
219
220 # 2-sigma ellipse
221 ex, ey = covariance_ellipse(pos_mean, pos_cov, n_std=2.0)
222 fig.add_trace(
223 go.Scatter(
224 x=ex,
225 y=ey,
226 mode="lines",
227 line=dict(color="rgba(0, 100, 255, 0.3)", width=1),
228 fill="toself",
229 fillcolor="rgba(0, 100, 255, 0.1)",
230 name="2σ covariance" if i == 0 else None,
231 showlegend=(i == 0),
232 )
233 )
234
235 fig.update_layout(
236 title="Kalman Filter Tracking with Covariance Ellipses",
237 xaxis_title="X Position",
238 yaxis_title="Y Position",
239 xaxis=dict(scaleanchor="y", scaleratio=1),
240 width=1000,
241 height=800,
242 )
243
244 return fig
245
246
247def plot_uncertainty_evolution(data: dict) -> go.Figure:
248 """
249 Plot how position and velocity uncertainties evolve over time.
250 """
251 covariances = data["covariances"]
252 dt = data["dt"]
253 n_steps = len(covariances)
254 time = np.arange(n_steps) * dt
255
256 # Extract position and velocity standard deviations
257 pos_x_std = np.array([np.sqrt(P[0, 0]) for P in covariances])
258 pos_y_std = np.array([np.sqrt(P[2, 2]) for P in covariances])
259 vel_x_std = np.array([np.sqrt(P[1, 1]) for P in covariances])
260 vel_y_std = np.array([np.sqrt(P[3, 3]) for P in covariances])
261
262 fig = make_subplots(
263 rows=2,
264 cols=1,
265 subplot_titles=["Position Uncertainty (1σ)", "Velocity Uncertainty (1σ)"],
266 shared_xaxes=True,
267 )
268
269 # Position uncertainties
270 fig.add_trace(
271 go.Scatter(
272 x=time, y=pos_x_std, mode="lines", name="σ_x", line=dict(color="blue")
273 ),
274 row=1,
275 col=1,
276 )
277 fig.add_trace(
278 go.Scatter(
279 x=time, y=pos_y_std, mode="lines", name="σ_y", line=dict(color="red")
280 ),
281 row=1,
282 col=1,
283 )
284
285 # Velocity uncertainties
286 fig.add_trace(
287 go.Scatter(
288 x=time,
289 y=vel_x_std,
290 mode="lines",
291 name="σ_vx",
292 line=dict(color="blue", dash="dash"),
293 ),
294 row=2,
295 col=1,
296 )
297 fig.add_trace(
298 go.Scatter(
299 x=time,
300 y=vel_y_std,
301 mode="lines",
302 name="σ_vy",
303 line=dict(color="red", dash="dash"),
304 ),
305 row=2,
306 col=1,
307 )
308
309 fig.update_xaxes(title_text="Time (s)", row=2, col=1)
310 fig.update_yaxes(title_text="Position std (m)", row=1, col=1)
311 fig.update_yaxes(title_text="Velocity std (m/s)", row=2, col=1)
312
313 fig.update_layout(
314 title="Filter Uncertainty Evolution Over Time",
315 width=1000,
316 height=600,
317 )
318
319 return fig
320
321
322def plot_estimation_errors(data: dict) -> go.Figure:
323 """
324 Plot estimation errors with uncertainty bounds.
325 """
326 true_states = data["true_states"]
327 estimates = data["estimates"]
328 covariances = data["covariances"]
329 dt = data["dt"]
330
331 # Compute errors
332 errors = estimates - true_states
333 n_steps = len(errors)
334 time = np.arange(n_steps) * dt
335
336 # Extract 2-sigma bounds
337 pos_x_2sigma = 2 * np.array([np.sqrt(P[0, 0]) for P in covariances])
338 pos_y_2sigma = 2 * np.array([np.sqrt(P[2, 2]) for P in covariances])
339
340 fig = make_subplots(
341 rows=2,
342 cols=1,
343 subplot_titles=["X Position Error", "Y Position Error"],
344 shared_xaxes=True,
345 )
346
347 # X error with bounds
348 fig.add_trace(
349 go.Scatter(
350 x=np.concatenate([time, time[::-1]]),
351 y=np.concatenate([pos_x_2sigma, -pos_x_2sigma[::-1]]),
352 fill="toself",
353 fillcolor="rgba(0, 100, 255, 0.2)",
354 line=dict(color="rgba(0,0,0,0)"),
355 name="±2σ bound",
356 ),
357 row=1,
358 col=1,
359 )
360 fig.add_trace(
361 go.Scatter(
362 x=time,
363 y=errors[:, 0],
364 mode="lines",
365 name="X error",
366 line=dict(color="blue"),
367 ),
368 row=1,
369 col=1,
370 )
371 fig.add_trace(
372 go.Scatter(
373 x=time,
374 y=np.zeros(n_steps),
375 mode="lines",
376 line=dict(color="black", dash="dash", width=1),
377 showlegend=False,
378 ),
379 row=1,
380 col=1,
381 )
382
383 # Y error with bounds
384 fig.add_trace(
385 go.Scatter(
386 x=np.concatenate([time, time[::-1]]),
387 y=np.concatenate([pos_y_2sigma, -pos_y_2sigma[::-1]]),
388 fill="toself",
389 fillcolor="rgba(255, 100, 0, 0.2)",
390 line=dict(color="rgba(0,0,0,0)"),
391 showlegend=False,
392 ),
393 row=2,
394 col=1,
395 )
396 fig.add_trace(
397 go.Scatter(
398 x=time, y=errors[:, 2], mode="lines", name="Y error", line=dict(color="red")
399 ),
400 row=2,
401 col=1,
402 )
403 fig.add_trace(
404 go.Scatter(
405 x=time,
406 y=np.zeros(n_steps),
407 mode="lines",
408 line=dict(color="black", dash="dash", width=1),
409 showlegend=False,
410 ),
411 row=2,
412 col=1,
413 )
414
415 fig.update_xaxes(title_text="Time (s)", row=2, col=1)
416 fig.update_yaxes(title_text="Error (m)", row=1, col=1)
417 fig.update_yaxes(title_text="Error (m)", row=2, col=1)
418
419 fig.update_layout(
420 title="Estimation Errors with 2σ Confidence Bounds",
421 width=1000,
422 height=600,
423 )
424
425 return fig
426
427
428def plot_animated_tracking(data: dict) -> go.Figure:
429 """
430 Create an animated visualization of the tracking process.
431 """
432 true_states = data["true_states"]
433 measurements = data["measurements"]
434 estimates = data["estimates"]
435 covariances = data["covariances"]
436 n_steps = len(measurements)
437
438 # Create frames for animation
439 frames = []
440
441 for k in range(1, n_steps + 1):
442 # True trajectory up to current time
443 true_trace = go.Scatter(
444 x=true_states[: k + 1, 0],
445 y=true_states[: k + 1, 2],
446 mode="lines",
447 line=dict(color="green", width=3),
448 name="True",
449 )
450
451 # Measurements up to current time
452 meas_trace = go.Scatter(
453 x=measurements[:k, 0],
454 y=measurements[:k, 1],
455 mode="markers",
456 marker=dict(color="black", size=6, symbol="x"),
457 name="Measurements",
458 )
459
460 # Estimates up to current time
461 est_trace = go.Scatter(
462 x=estimates[: k + 1, 0],
463 y=estimates[: k + 1, 2],
464 mode="lines+markers",
465 line=dict(color="blue", width=2),
466 marker=dict(size=4),
467 name="Estimate",
468 )
469
470 # Current covariance ellipse
471 pos_mean = np.array([estimates[k, 0], estimates[k, 2]])
472 pos_cov = np.array(
473 [
474 [covariances[k][0, 0], covariances[k][0, 2]],
475 [covariances[k][2, 0], covariances[k][2, 2]],
476 ]
477 )
478 ex, ey = covariance_ellipse(pos_mean, pos_cov, n_std=2.0)
479
480 ellipse_trace = go.Scatter(
481 x=ex,
482 y=ey,
483 mode="lines",
484 line=dict(color="rgba(0, 100, 255, 0.5)", width=2),
485 fill="toself",
486 fillcolor="rgba(0, 100, 255, 0.2)",
487 name="2σ covariance",
488 )
489
490 frames.append(
491 go.Frame(
492 data=[true_trace, meas_trace, est_trace, ellipse_trace],
493 name=str(k),
494 )
495 )
496
497 # Initial frame
498 fig = go.Figure(
499 data=frames[0].data,
500 frames=frames,
501 )
502
503 # Add animation controls
504 fig.update_layout(
505 title="Animated Kalman Filter Tracking",
506 xaxis=dict(
507 range=[
508 min(true_states[:, 0].min(), estimates[:, 0].min()) - 10,
509 max(true_states[:, 0].max(), estimates[:, 0].max()) + 10,
510 ],
511 title="X Position",
512 scaleanchor="y",
513 scaleratio=1,
514 ),
515 yaxis=dict(
516 range=[
517 min(true_states[:, 2].min(), estimates[:, 2].min()) - 10,
518 max(true_states[:, 2].max(), estimates[:, 2].max()) + 10,
519 ],
520 title="Y Position",
521 ),
522 updatemenus=[
523 dict(
524 type="buttons",
525 showactive=False,
526 y=1.15,
527 x=0.5,
528 xanchor="center",
529 buttons=[
530 dict(
531 label="▶ Play",
532 method="animate",
533 args=[
534 None,
535 dict(
536 frame=dict(duration=100, redraw=True),
537 fromcurrent=True,
538 mode="immediate",
539 ),
540 ],
541 ),
542 dict(
543 label="⏸ Pause",
544 method="animate",
545 args=[
546 [None],
547 dict(
548 frame=dict(duration=0, redraw=False),
549 mode="immediate",
550 ),
551 ],
552 ),
553 ],
554 )
555 ],
556 sliders=[
557 dict(
558 active=0,
559 steps=[
560 dict(
561 args=[
562 [str(k)],
563 dict(frame=dict(duration=0, redraw=True), mode="immediate"),
564 ],
565 label=str(k),
566 method="animate",
567 )
568 for k in range(1, n_steps + 1)
569 ],
570 x=0.1,
571 len=0.8,
572 xanchor="left",
573 y=0,
574 yanchor="top",
575 currentvalue=dict(
576 prefix="Time step: ",
577 visible=True,
578 xanchor="center",
579 ),
580 )
581 ],
582 width=1000,
583 height=800,
584 )
585
586 return fig
587
588
589def main():
590 """Run filter uncertainty visualization examples."""
591 print("Filter Uncertainty Visualization Examples")
592 print("=" * 50)
593
594 # Simulate tracking scenario
595 print("\nSimulating tracking scenario...")
596 data = simulate_tracking_scenario(n_steps=50, dt=1.0)
597 print(f" Generated {len(data['measurements'])} time steps")
598
599 # 1. Tracking with ellipses
600 print("\n1. Generating tracking with covariance ellipses...")
601 fig1 = plot_tracking_with_ellipses(data)
602 fig1.write_html(
603 str(OUTPUT_DIR / "filter_viz_tracking_ellipses.html"),
604 include_plotlyjs="cdn",
605 div_id="filter_viz_tracking_ellipses",
606 )
607 print(" Saved to filter_viz_tracking_ellipses.html")
608
609 # 2. Uncertainty evolution
610 print("\n2. Generating uncertainty evolution plot...")
611 fig2 = plot_uncertainty_evolution(data)
612 fig2.write_html(
613 str(OUTPUT_DIR / "filter_viz_uncertainty_evolution.html"),
614 include_plotlyjs="cdn",
615 div_id="filter_viz_uncertainty_evolution",
616 )
617 print(" Saved to filter_viz_uncertainty_evolution.html")
618
619 # 3. Estimation errors
620 print("\n3. Generating estimation error plot...")
621 fig3 = plot_estimation_errors(data)
622 fig3.write_html(
623 str(OUTPUT_DIR / "filter_viz_estimation_errors.html"),
624 include_plotlyjs="cdn",
625 div_id="filter_viz_estimation_errors",
626 )
627 print(" Saved to filter_viz_estimation_errors.html")
628
629 # 4. Animated tracking
630 print("\n4. Generating animated tracking visualization...")
631 fig4 = plot_animated_tracking(data)
632 fig4.write_html(
633 str(OUTPUT_DIR / "filter_viz_animated.html"),
634 include_plotlyjs="cdn",
635 div_id="filter_viz_animated",
636 )
637 print(" Saved to filter_viz_animated.html")
638
639 # Show all figures
640 print("\nOpening visualizations in browser...")
641 if SHOW_PLOTS:
642 fig1.show()
643 if SHOW_PLOTS:
644 fig2.show()
645 if SHOW_PLOTS:
646 fig3.show()
647 if SHOW_PLOTS:
648 fig4.show()
649
650 print("\nDone!")
651
652
653if __name__ == "__main__":
654 main()
Running the Example
python examples/filter_uncertainty_visualization.py
See Also
Kalman Filter Comparison - Kalman filter variants
Particle Filters - Particle filter uncertainty representation