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

Running the Example

python examples/filter_uncertainty_visualization.py

See Also