貝葉斯濾波

1.理論

貝葉斯推斷方法的關(guān)鍵是任何推斷都必須且只須根據(jù)后驗(yàn)分布，而不能再涉及樣本分布

2.貝葉斯濾波的基本方法：

優(yōu)缺點(diǎn)：

• 優(yōu)點(diǎn)：可以有效濾除噪聲，得到比較精準(zhǔn)的狀態(tài)估計(jì)

• 缺點(diǎn)：需要做無窮積分，大多數(shù)情況下沒有解析解

卡爾曼濾波是它的改進(jìn)版）

實(shí)例

利用opencv自帶的kalmanfilter類實(shí)現(xiàn)對(duì)鼠標(biāo)軌跡的跟蹤

步驟

卡爾曼濾波器算法分為兩個(gè)階段：

1. 預(yù)測(cè)：使用由當(dāng)前點(diǎn)計(jì)算的協(xié)方差來估計(jì)目標(biāo)的新位置

2. 更新：記錄目標(biāo)的位置，并為下一次循環(huán)計(jì)算修正協(xié)方差

kalman = cv2.KalmanFilter(4, 2)

表示Kalman濾波器轉(zhuǎn)移矩陣維度為4，測(cè)量矩陣維度為2

（因?yàn)闋顟B(tài)量有4個(gè)：x，y方向分別的位移和速度，可觀測(cè)的量有2個(gè)：x，y方向的位移）

測(cè)量矩陣：

轉(zhuǎn)移矩陣：

例：

kalman.measuremenMatrix = 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)

過程噪聲和測(cè)量噪聲以一個(gè)經(jīng)驗(yàn)值估計(jì)（來源csdn）

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

具體操作：

3. 初始化數(shù)組，存放測(cè)量坐標(biāo)和鼠標(biāo)運(yùn)動(dòng)預(yù)測(cè)的數(shù)據(jù)

4. 傳遞進(jìn)當(dāng)前測(cè)量的坐標(biāo)值

5. 修正卡爾曼濾波預(yù)測(cè)結(jié)果

6. 調(diào)用kalman類的predict，得到預(yù)測(cè)值矩陣（用來估算目標(biāo)位置

7. 將上一次測(cè)量值和當(dāng)前測(cè)量值連線；將上一次預(yù)測(cè)值和當(dāng)前預(yù)測(cè)值連線

注意：

• Kalman這個(gè)類需要初始化下面變量：

轉(zhuǎn)移矩陣，測(cè)量矩陣，控制向量(沒有的話，就是0)，過程噪聲協(xié)方差矩陣，測(cè)量噪聲協(xié)方差矩陣，后驗(yàn)錯(cuò)誤協(xié)方差矩陣，前一狀態(tài)校正后的值，當(dāng)前觀察值。

• cv2.KalmanFilter(4,2)表示轉(zhuǎn)移矩陣維度為4，測(cè)量矩陣維度為2

• 卡爾曼濾波模型假設(shè)k時(shí)刻的真實(shí)狀態(tài)是從(k ? 1)時(shí)刻的狀態(tài)演化而來，符合下式：

        X(k) = F(k) * X(k-1) + B(k)*U(k) + W（k）

其中

F(k) 是作用在xk?1上的狀態(tài)變換模型（/矩陣/矢量）。

B(k) 是作用在控制器向量uk上的輸入－控制模型。

W(k) 是過程噪聲，并假定其符合均值為零，協(xié)方差矩陣為Qk的多元正態(tài)分布。

完整代碼：文章來源地址http://www.zghlxwxcb.cn/news/detail-630076.html

import cv2
import numpy as np

# 創(chuàng)建一個(gè)畫圖區(qū)域
frame = np.zeros((700, 700, 3), np.uint8) 

# 初始化測(cè)量坐標(biāo)和鼠標(biāo)運(yùn)動(dòng)預(yù)測(cè)的數(shù)組
last_measurement = current_measurement = np.array((2, 1), np.float32)
last_prediction = current_prediction = np.zeros((2, 1), np.float32)

# 用來繪制跟蹤結(jié)果
def mousemove(event, x, y, s, p):
    global frame, current_measurement, measurements, last_measurement, mcurrent_prediction, last_prediction
    # 把當(dāng)前預(yù)測(cè)存儲(chǔ)為上一次預(yù)測(cè)
    last_prediction = current_prediction 
    # 把當(dāng)前測(cè)量存儲(chǔ)為上一次測(cè)量
    last_measurement = current_measurement

   
    current_measurement = np.array([[np.float32(x)], [np.float32(y)]]) 
    # 用當(dāng)前測(cè)量來校正卡爾曼濾波器
    kalman.correct(current_measurement) 
    # 計(jì)算卡爾曼預(yù)測(cè)值，作為當(dāng)前預(yù)測(cè)
    current_prediction = kalman.predict() 

    # 上一次測(cè)量坐標(biāo)
    lmx, lmy = last_measurement[0], last_measurement[1] 
    # 當(dāng)前測(cè)量坐標(biāo)
    cmx, cmy = current_measurement[0], current_measurement[1] 
    # 上一次預(yù)測(cè)坐標(biāo)
    lpx, lpy = last_prediction[0], last_prediction[1] 
    # 當(dāng)前預(yù)測(cè)坐標(biāo)
    cpx, cpy = current_prediction[0], current_prediction[1] 

    # 繪制從上一次測(cè)量到當(dāng)前測(cè)量以及從上一次預(yù)測(cè)到當(dāng)前預(yù)測(cè)的兩條線
    # 藍(lán)色為測(cè)量值
    cv2.line(frame, (int(lmx), int(lmy)), (int(cmx), int(cmy)), (255, 0, 0)) 
    # 粉色為預(yù)測(cè)值
    cv2.line(frame, (int(lpx), int(lpy)), (int(cpx), int(cpy)), (255, 0, 255)) 

cv2.namedWindow("kalman_tracker")
cv2.setMouseCallback("kalman_tracker", mousemove)

# 4：狀態(tài)數(shù)，包括（x，y，dx，dy）坐標(biāo)及速度（每次移動(dòng)的距離）；2：觀測(cè)量，能看到的是坐標(biāo)值
kalman = cv2.KalmanFilter(4, 2)
# 系統(tǒng)測(cè)量矩陣
kalman.measurementMatrix = np.array([[1, 0, 0, 0], [0, 1, 0, 0]], np.float32)
# 狀態(tài)轉(zhuǎn)移矩陣
kalman.transitionMatrix = np.array([[1, 0, 1, 0], [0, 1, 0, 1], [0, 0, 1, 0], [0, 0, 0, 1]], np.float32) 
# 系統(tǒng)過程噪聲協(xié)方差
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)
    key = cv2.waitKey(1) & 0xFF
    if key == ord('q'):
        break
cv2.destroyAllWindows()