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

Running the Example

python examples/dynamic_models_demo.py

See Also