## 编程知识 cdmana.com

### Simple implementation of Kalman filter

In the field of target tracking ,Kalman Filter is a very common method .

Take tracking the position and velocity of an object in a two-dimensional plane as an example , Explain how to implement a simple Kalman tracker .

Specific use OpenCV Medium `KalmanFilter` Class to achieve .

## 1. Parameter initialization

``````kalman = cv2.KalmanFilter(4,2)
``````

Express Kalman The dimension of filter transfer matrix is 4, The dimension of measurement matrix is 2.

Because state variables include 4 individual （ Namely x、y Displacement and velocity in direction ）, The observable quantities are 2 individual （ Namely x、y Displacement in direction ）.

``````kalman.measurementMatrix = np.array([[1,0,0,0],[0,1,0,0]],np.float32)
``````

The measurement matrix and its meaning are ：

``````kalman.transitionMatrix = np.array([[1,0,1,0],[0,1,0,1],[0,0,1,0],[0,0,0,1]], np.float32)
``````

The transfer matrix and its meaning are ：

``````kalman.processNoiseCov = np.array([[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]], np.float32) * 0.003
kalman.measurementNoiseCov = np.array([[1,0],[0,1]], np.float32) * 0.5
``````

Process noise and measurement noise are estimated by an empirical value .

## 2. Generate test data

Test data are generated by superposition of trigonometric function and random disturbance .

``````def data_generator(length=100):
dxy = []
xy = []
last_xy = [0, 0]
for i in range(length):
x_base = 5 - 5 * math.cos(2 * i * math.pi / length)
y_base = 50 - 50 * math.cos(2 * i * math.pi / length)
x_noise = 1 * (random()-0.5)
y_noise = 20 * (random()-0.5)
dx_base = math.sin(2 * i * math.pi / length)
dy_base = 30 * math.sin(2 * i * math.pi / length)
dx_noise = 1 * (random()-0.5)
dy_noise = 5 * (random()-0.5)
cur_xy = [x_base + x_noise + dx_base + dx_noise, \
y_base + y_noise + dy_base + dy_noise]
cur_dxy = [cur_xy[0]-last_xy[0], cur_xy[1]-last_xy[1]]
xy.append(cur_xy)
dxy.append(cur_dxy)
last_xy = cur_xy
return np.array(dxy, dtype=np.float32), \
np.array(xy, dtype=np.float32)
``````

## 3. function

The core is kalman Two approaches ：

• `correct` Update the current measurement ;
• `predict` Predicts the value of the next frame .
``````length = 100
dxy, xy = data_generator2(length)
dxy_pred = []
xy_pred = []
for i in range(length):
kalman.correct(xy[i])
current_prediction = kalman.predict()
xy_pred.append(current_prediction[:2, 0])
dxy_pred.append(current_prediction[2:, 0])
dxy_pred = np.stack(dxy_pred, axis=0)
xy_pred = np.stack(xy_pred, axis=0)
``````

## 4. visualization

utilize `Matplotlib` Visualize the results as shown in the figure below ：

The visualization code is as follows ：

``````plot_image((xy, dxy, xy_pred, dxy_pred))

def plot_image(inputs):
xy, dxy, xy_pred, dxy_pred = inputs

fig, axes = plt.subplots(2, 2)
fig.set_size_inches(18, 9)
axes[0, 0].plot(xy[:,0], color='red', label='Measured')
axes[0, 0].plot(xy_pred[:,0], color='blue', label='Predicted')
axes[0, 1].plot(xy[:,1], color='red', label='Measured')
axes[0, 1].plot(xy_pred[:,1], color='blue', label='Predicted')
axes[1, 0].plot(dxy[:,0], color='red', label='Measured')
axes[1, 0].plot(dxy_pred[:,0], color='blue', label='Predicted')
axes[1, 1].plot(dxy[:,1], color='red', label='Measured')
axes[1, 1].plot(dxy_pred[:,1], color='blue', label='Predicted')

axes[0, 0].set_title('Distance - X',loc='center',fontstyle='normal')
axes[0, 1].set_title('Distance - Y',loc='center',fontstyle='normal')
axes[1, 0].set_title('Speed - X',loc='center',fontstyle='normal')
axes[1, 1].set_title('Speed - Y',loc='center',fontstyle='normal')
axes[0, 0].legend()
axes[0, 1].legend()
axes[1, 0].legend()
axes[1, 1].legend()
plt.show()
return
``````

https://cdmana.com/2021/01/20210131210132022S.html

Scroll to Top