預備知識：

卡爾曼濾波的理論知識：

具體的理論知識可參考以下博文，非常感謝相關博主的貢獻：

以一個滑鼠追蹤的任務分析兩種卡爾曼濾波的實現方式：

（一）opencv自帶的cv2.KalmanFilter

該卡爾曼濾波器演算法分為兩個階段：
預測predict()：卡爾曼濾波器使用由當前點計算的協方差來估計目標的新位置。
更新correct()：卡爾曼濾波器記錄目標的位置，併為下一次迴圈計算修正協方差。
將繪製一個空幀和兩條線：一條線對應於滑鼠的實際運動，另一條對應於卡爾曼濾波器預測的軌跡。

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,measurements,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.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()
 

執行結果：

卡爾曼相關解讀：

stateNum=4;//狀態數，包括（x，y，dx，dy）座標及速度（每次移動的距離）
measureNum=2;//觀測量，能看到的是座標值，當然也可以自己計算速度

（二）pykalman演算法庫

from pykalman import KalmanFilter

Apply the Kalman Filter to estimate the hidden state at time  for  given observations up to and including time t. Observations are assumed to correspond to times 

The output of this method corresponding to time  can be used in KalmanFilter.filter_update() for online updating.

Perform a one-step update to estimate the state at time  give an observation at time  and the previous estimate for time  given observations from times . This method is useful if one wants to track an object with streaming observations.

簡而言之，就是說KalmanFilter.filter()主要用於前面觀測狀態皆給出的情況，KalmanFilter.filter_update()主要用於已知前一時刻的狀態估計量（包括t時刻的預測狀態及協方差的預測值）的時候，要根據t+1時刻的觀測值估計t+1時刻的狀態估計量，所以用於需要實時估計的情況，如滑鼠跟蹤。

import cv2
import numpy as np
from pykalman import KalmanFilter

frame = np.zeros((800,800,3),np.uint8)
kf = KalmanFilter(transition_matrices=np.array([[1,0,1,0], [0,1,0,1],[0,0,1,0], [0,0,0,1]]),
                  observation_matrices =np.array([[1,0,0,0],[0,1,0,0]]),
                  transition_covariance= 0.03*np.eye(4))
#transition_matrices：公式中的A
# observation_matrices：公式中的H
#transition_covariance：公式中的Q

t=0


#狀態值為x_t=[x,y,dx,dy],其中(x,y)為滑鼠當前位置，（dx,dy）指速度分量
#直接獲得的觀測為位置(x,y)

def mousemove(event,x,y,s,p):
    global t, filtered_state_means0,filtered_state_covariances0,lmx,lmy,lpx,lpy
    current_measurement = np.array([np.float32(x),np.float32(y)])
    cmx, cmy = current_measurement[0], current_measurement[1]
    if t == 0:
        filtered_state_means0=np.array([0.0,0.0,0.0,0.0])
        filtered_state_covariances0=np.eye(4)
        lmx, lmy = 0.0, 0.0
        lpx, lpy = 0.0, 0.0

    filtered_state_means, filtered_state_covariances= (kf.filter_update( filtered_state_means0，filtered_state_covariances0,current_measurement))
    cpx,cpy= filtered_state_means[0], filtered_state_means[1]
    #繪製測量值軌跡（綠色）
    cv2.line(frame,(int(lmx),int(lmy)),(int(cmx),int(cmy)),(0,100,0))
    #繪製預測值軌跡（紅色）
    cv2.line(frame,(int(lpx),int(lpy)),(int(cpx),int(cpy)),(0,0,200))
    filtered_state_means0, filtered_state_covariances0=filtered_state_means, filtered_state_covariances
    t=t+1
    lpx, lpy = filtered_state_means[0], filtered_state_means[1]
    lmx, lmy =current_measurement[0], current_measurement[1]
cv2.namedWindow("kalman_tracker")
cv2.setMouseCallback("kalman_tracker",mousemove)
while True:
    cv2.imshow("kalman_tracker",frame)
    if (cv2.waitKey(30) & 0xff) == 27:
        break

cv2.destroyAllWindows()

執行結果：