Dynamic Models

This example demonstrates dynamic models and state transition matrices used in Kalman filtering and target tracking.

Overview

Dynamic models describe how a target’s state evolves over time. They are fundamental to:

  • Kalman filtering: Prediction step requires state transition

  • Target tracking: Motion models for different target types

  • Navigation: INS mechanization and error propagation

  • Simulation: Generating realistic target trajectories

Key Concepts

State Transition Matrix (Phi)

The discrete-time matrix that propagates state from time k to k+1: x[k+1] = Phi * x[k] + process_noise

Process Noise Covariance (Q)

Captures uncertainty in the motion model due to unknown accelerations or model mismatch.

Continuous vs Discrete Time

  • Continuous: differential equations (dx/dt = F*x)

  • Discrete: difference equations (x[k+1] = Phi*x[k])

  • Conversion: Phi = exp(F*dt)

State Estimation: The state transition matrix propagates the state estimate and covariance through time, shown as uncertainty ellipses.

Models Demonstrated

Constant Velocity (CV)

  • State: [x, vx, y, vy, z, vz]

  • Assumes constant velocity between updates

  • Process noise models unknown accelerations

Drift Functions

  • Continuous-time rate of change

  • Position changes at velocity rate

  • Velocity remains constant (for CV model)

3D Tracking: Dynamic models enable prediction of 3D target trajectories using the state transition matrix.

Code Highlights

The example demonstrates:

  • State transition matrix computation with f_constant_velocity()

  • Process noise covariance with diffusion_constant_velocity()

  • Drift function evaluation with drift_constant_velocity()

  • Continuous to discrete time conversion

Source Code

  1"""
  2Demonstration of dynamic models and state transition matrices.
  3
  4This example shows:
  5- Continuous and discrete-time system models
  6- State transition matrices (Phi matrices)
  7- Process noise covariance (Q matrices)
  8- Kalman filter compatibility
  9"""
 10
 11from pathlib import Path
 12
 13import numpy as np
 14import plotly.graph_objects as go
 15
 16from pytcl.dynamic_models.continuous_time import (
 17    diffusion_constant_velocity,
 18)
 19from pytcl.dynamic_models.discrete_time import f_constant_velocity
 20
 21SHOW_PLOTS = True
 22
 23
 24def demo_state_transition_matrix() -> None:
 25    """Demonstrate state transition matrix properties."""
 26    print("\n" + "=" * 60)
 27    print("State Transition Matrices")
 28    print("=" * 60)
 29
 30    dt = 0.1  # Time step
 31
 32    # Get state transition matrix for constant velocity model
 33    # Returns 6x6 matrix for 3D position/velocity
 34    phi = f_constant_velocity(dt)
 35
 36    print(f"\nTime step: {dt}s")
 37    print(f"State transition matrix (Phi) shape: {phi.shape}")
 38    print("Matrix (first 3x3 block):")
 39    print(phi[:3, :3])
 40
 41    # The matrix has a block structure
 42    # [I  dt*I]
 43    # [0   I  ]
 44    # where I is 3x3 for position and velocity in 3D
 45
 46
 47def demo_process_noise() -> None:
 48    """Demonstrate process noise matrices."""
 49    print("\n" + "=" * 60)
 50    print("Process Noise Covariance Matrices")
 51    print("=" * 60)
 52
 53    dt = 0.1
 54    sigma_v = 1.0  # Process noise standard deviation
 55
 56    # Get process noise covariance matrix
 57    q_matrix = diffusion_constant_velocity(dt, sigma_v)
 58
 59    print(f"\nTime step: {dt}s")
 60    print(f"Process noise std: {sigma_v} m/s")
 61    print(f"Process noise matrix (Q) shape: {q_matrix.shape}")
 62
 63    # Properties
 64    print(f"\nProcess noise norm: {np.linalg.norm(q_matrix):.6f}")
 65
 66
 67def demo_drift_matrix() -> None:
 68    """Demonstrate drift (mean) functions."""
 69    print("\n" + "=" * 60)
 70    print("Drift Functions")
 71    print("=" * 60)
 72
 73    # Drift function takes a state vector and returns rate of change
 74    # For constant velocity, position changes at velocity rate
 75    state = np.array([0.0, 1.0, 0.0, 2.0, 0.0, 3.0])  # 3D pos/vel
 76
 77    from pytcl.dynamic_models.continuous_time.dynamics import (
 78        drift_constant_velocity as drift_cv,
 79    )
 80
 81    drift_result = drift_cv(state)
 82
 83    print(f"\nExample state (3D position + velocity):")
 84    print(f"  Position: {state[0::2]}")
 85    print(f"  Velocity: {state[1::2]}")
 86    print(f"\nDrift (rate of change) result:")
 87    print(f"  {drift_result}")
 88
 89    # Expected: [vel_1, 0, vel_2, 0, vel_3, 0]
 90    # showing that position changes at velocity rate and velocity doesn't change
 91
 92
 93def demo_continuous_to_discrete_conversion() -> None:
 94    """Demonstrate continuous to discrete time conversion principle."""
 95    print("\n" + "=" * 60)
 96    print("Continuous to Discrete Conversion")
 97    print("=" * 60)
 98
 99    # For a constant velocity model in 1D:
100    # Continuous: dx/dt = v, dv/dt = 0
101    # Or in matrix form: [dx/dt; dv/dt] = [0 1; 0 0] * [x; v]
102
103    # Discrete approximation: state[k+1] = Phi * state[k]
104    # where Phi = exp(F * dt) ≈ I + F*dt for small dt
105
106    F = np.array([[0.0, 1.0], [0.0, 0.0]])  # Continuous F matrix
107    dts = [0.05, 0.1, 0.2, 0.5]
108
109    print("\nContinuous F matrix (1D constant velocity):")
110    print(F)
111
112    print("\nDiscrete Phi matrices (Phi ≈ I + F*dt):")
113    print("Time Step | Phi[0,1] Value")
114    print("-" * 30)
115
116    phi_values = []
117    for dt_val in dts:
118        # For constant velocity: Phi = [[1, dt], [0, 1]]
119        phi_approx = np.eye(2) + F * dt_val
120        phi_values.append(phi_approx[0, 1])
121        print(f"{dt_val:>8} | {phi_approx[0, 1]:>14.6f}")
122
123    # Plot
124    if SHOW_PLOTS:
125        fig = go.Figure()
126
127        fig.add_trace(
128            go.Scatter(
129                x=dts,
130                y=phi_values,
131                mode="lines+markers",
132                name="Phi[0,1]",
133                line=dict(color="purple", width=2),
134                marker=dict(size=8),
135            )
136        )
137
138        fig.update_layout(
139            title="Discrete State Transition Element vs Time Step",
140            xaxis_title="Time Step (s)",
141            yaxis_title="Phi[0,1] Value",
142            height=400,
143        )
144
145        if SHOW_PLOTS:
146            fig.show()
147        else:
148            fig.write_html(str(OUTPUT_DIR / "dynamic_models_demo.html"))
149
150
151def main() -> None:
152    """Run all demonstrations."""
153    print("\n" + "=" * 60)
154    print("Dynamic Models Demonstration")
155    print("=" * 60)
156
157    demo_state_transition_matrix()
158    demo_process_noise()
159    demo_drift_matrix()
160    demo_continuous_to_discrete_conversion()
161
162    print("\n" + "=" * 60)
163    print("Demonstration Complete")
164    print("=" * 60)
165
166
167OUTPUT_DIR = Path("docs/_static/images/examples")
168OUTPUT_DIR.mkdir(parents=True, exist_ok=True)
169
170if __name__ == "__main__":
171    main()

Running the Example

python examples/dynamic_models_demo.py

See Also

  • kalman_filter_comparison - Kalman filter implementations

  • multi_target_tracking - Multi-target tracking

  • tracking_3d - 3D tracking example