Kalman濾波器的簡單實現

在目標跟蹤領域，Kalman濾波器是一個很經常使用的方法。

以在二維平面中跟蹤一個物體的位置和速度爲例，說明若是實現一個簡單的Kalman跟蹤器。

具體使用OpenCV中的KalmanFilter類來實現。

1. 參數初始化

kalman = cv2.KalmanFilter(4,2)

表示Kalman濾波器轉移矩陣維度爲4，測量矩陣維度爲2。

由於狀態量包括4個（分別是x、y方向的位移和速度），可觀測的量有2個（分別是x、y方向的位移）。

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

測量矩陣及其含義爲：

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

轉移矩陣及其含義爲：

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

過程噪聲和測量噪聲以一個經驗值來估計。

2. 生成測試數據

用三角函數疊加隨機擾動的方式來生成測試數據。

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. 運行

核心就是kalman的兩個方法：

• correct更新當前測量值；
• predict預測下一幀的值。

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. 可視化

利用Matplotlib將結果可視化後以下圖所示：

可視化部分代碼以下所示：

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