卡爾曼濾波簡介

卡爾曼在利用觀測資料估計系統狀態時，可以濾除觀測時存在的噪聲，因此這一過程也被看作是一個濾波過程。

跟蹤滑鼠

綠色為測量到的滑鼠座標（位置）

紅色為卡爾曼濾波器預測的滑鼠座標（位置）

# -*- coding: UTF-8 -*-
# ! /usr/bin/python

import cv2
import numpy as np

# 建立一個大小800*800的空幀
frame = np.zeros((800, 800, 3), np.uint8)
# 初始化測量座標和滑鼠運動預測的陣列
last_measurement = current_measurement = np.array((2, 1), np.float32)
last_predicition = current_prediction = np.zeros((2, 1), np.float32)

'''
    mousemove()函式在這裡的作用就是傳遞X,Y的座標值，便於對軌跡進行卡爾曼濾波
'''
def mousemove(event, x, y, s, p):
    # 定義全域性變數
    global frame, current_measurement, last_measurement, current_prediction, last_prediction
    # 初始化
    last_measurement = current_measurement
    last_prediction = current_prediction
    # 傳遞當前測量座標值
    current_measurement = np.array([[np.float32(x)], [np.float32(y)]])
    # 用來修正卡爾曼濾波的預測結果
    kalman.correct(current_measurement) # 用當前測量來校正卡爾曼濾波器
    # 呼叫kalman這個類的predict方法得到狀態的預測值矩陣，用來估算目標位置
    current_prediction = kalman.predict()
    # 上一次測量值
    lmx, lmy = last_measurement[0], last_measurement[1]
    # 當前測量值
    cmx, cmy = current_measurement[0], current_measurement[1]
    # 上一次預測值
    lpx, lpy = last_prediction[0], last_prediction[1]
    # 當前預測值
    cpx, cpy = current_prediction[0], current_prediction[1]
    # 繪製測量值軌跡（綠色）
    cv2.line(frame, (lmx, lmy), (cmx, cmy), (0, 100, 0))
    # 繪製預測值軌跡（紅色）
    cv2.line(frame, (lpx, lpy), (cpx, cpy), (0, 0, 200))

cv2.namedWindow("kalman_tracker")
# 呼叫函式處理滑鼠事件，具體事件必須由回撥函式的第一個引數來處理，該引數確定觸發事件的型別（點選和移動）
'''
void setMousecallback(const string& winname, MouseCallback onMouse, void* userdata=0)
       winname:視窗的名字
       onMouse:滑鼠響應函式，回撥函式。指定窗口裡每次滑鼠時間發生的時候，被呼叫的函式指標。
                這個函式的原型應該為void on_Mouse(int event, int x, int y, int flags, void* param);
       userdate：傳給回撥函式的引數

 void on_Mouse(int event, int x, int y, int flags, void* param);
        event是 CV_EVENT_*變數之一
        x和y是滑鼠指標在影象座標系的座標（不是視窗座標系）
        flags是CV_EVENT_FLAG的組合， param是使用者定義的傳遞到setMouseCallback函式呼叫的引數。
    常用的event：
        CV_EVENT_MOUSEMOVE
        CV_EVENT_LBUTTONDOWN
        CV_EVENT_RBUTTONDOWN
        CV_EVENT_LBUTTONUP
        CV_EVENT_RBUTTONUP
        和標誌位flags有關的：
        CV_EVENT_FLAG_LBUTTON
'''
cv2.setMouseCallback("kalman_tracker", mousemove)
'''
Kalman這個類需要初始化下面變數：
轉移矩陣，測量矩陣，控制向量(沒有的話，就是0)，
過程噪聲協方差矩陣，測量噪聲協方差矩陣，
後驗錯誤協方差矩陣，前一狀態校正後的值，當前觀察值。
    在此cv2.KalmanFilter(4,2)表示轉移矩陣維度為4，測量矩陣維度為2

卡爾曼濾波模型假設k時刻的真實狀態是從(k − 1)時刻的狀態演化而來，符合下式：
            X(k) = F(k) * X(k-1) + B(k)*U(k) + W（k）
其中
F(k)  是作用在xk−1上的狀態變換模型（/矩陣/向量）。 
B(k)  是作用在控制器向量uk上的輸入－控制模型。 
W(k)  是過程噪聲，並假定其符合均值為零，協方差矩陣為Qk的多元正態分佈。
'''
kalman = cv2.KalmanFilter(4, 2) # 建立kalman濾波器 dynam_params：狀態空間的維數；measure_param：測量值的維數；control_params：控制向量的維數，預設為0。由於這裡該模型中並沒有控制變數，因此也為0。
# 設定測量矩陣
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.03

while True:
    cv2.imshow("kalman_tracker", frame)
    if (cv2.waitKey(30) & 0xff) == 27:
        break

cv2.destroyAllWindows()
 

建立滑鼠運動的模型，至少有兩個狀態變數：滑鼠位置x,y，也可以是四個狀態變數：位置x,y和速度vx，vy。兩個測量變數：滑鼠位置x,y。