filterpy_1d_kalman_filter_constant_velocity_tracking_quickstart.py

1import numpy as np
2from filterpy.kalman import KalmanFilter
3from filterpy.common import Q_discrete_white_noise
4
5# 1. Construct the filter
6f = KalmanFilter(dim_x=2, dim_z=1)
7
8# 2. Assign initial values
9f.x = np.array([[0.],    # position
10                [0.]])   # velocity
11
12f.F = np.array([[1., 1.],
13                [0., 1.]])
14
15f.H = np.array([[1., 0.]])
16
17f.P *= 1000.
18
19f.R = 5
20
21f.Q = Q_discrete_white_noise(dim=2, dt=0.1, var=0.13)
22
23# 3. Perform the Kalman Filter loop
24while True:
25    # Get a measurement (this would normally come from your sensor)
26    z = 1.0 
27    
28    # Predict step
29    f.predict()
30    
31    # Update step
32    f.update(z)
33
34    # Use the estimate
35    print(f.x)
36    break # Break included for demonstration purposes