1.0 什麼是卡爾曼模型

理論推導過於複雜，看個知乎上的解釋吧  假設你有兩個感測器，測的是同一個訊號。可是它們每次的讀數都不太一樣，怎麼辦？ 取平均。  再假設你知道其中貴的那個感測器應該準一些，便宜的那個應該差一些。那有比取平均更好的辦法嗎？ 加權平均。  怎麼加權？假設兩個感測器的誤差都符合正態分佈，假設你知道這兩個正態分佈的方差，用這兩個方差值，（此處省略若干數學公式），你可以得到一個“最優”的權重。  接下來，重點來了：假設你只有一個感測器，但是你還有一個數學模型。模型可以幫你算出一個值，但也不是那麼準。怎麼辦？ 把模型算出來的值，和感測器測出的值，（就像兩個感測器那樣），取加權平均。

總而言之，kalman濾波用在當測量值與模型預測值均不準確的情況下，用來計算預測真值的一種濾波方法。這在目標識別與追蹤任務中經常用到。

2.0 Python實戰–2維位置跟蹤

在這個例子中，我們知道2維位置測量值mes_x，mes_y。並且我們的先驗模型預測值為model_x，model_y。我們通過kalman filter來預測真值的大小。首先來建立2維運動追蹤模型，即上的model。在這個例子中，我們的model位置預測值設定為n時刻的位置由n-1時刻的位置加上一個隨機噪聲決定，如下圖所示。 

python 程式如下

import cv2
import numpy as np
import matplotlib.pyplot as plt

pos = np.array([
        [10,    50],
        [12,    49],    
        [11,    52],     
        [13,    52.2],     
        [12.9,  50]], np.float32)    

'''
它有3個輸入引數，dynam_params：狀態空間的維數，這裡為2；measure_param：測量值的維數，這裡也為2; control_params：控制向量的維數，預設為0。由於這裡該模型中並沒有控制變數，因此也為0。
'''
kalman = cv2.KalmanFilter(2,2)

kalman.measurementMatrix = np.array([[1,0],[0,1]],np.float32)
kalman.transitionMatrix = np.array([[1,0],[0,1]], np.float32)
kalman.processNoiseCov = np.array([[1,0],[0,1]], np.float32) * 1e-3
kalman.measurementNoiseCov = np.array([[1,0],[0,1]], np.float32) * 0.01
'''
kalman.measurementNoiseCov為測量系統的協方差矩陣，方差越小，預測結果越接近測量值，kalman.processNoiseCov為模型系統的噪聲，噪聲越大，預測結果越不穩定，越容易接近模型系統預測值，且單步變化越大，相反，若噪聲小，則預測結果與上個計算結果相差不大。
'''

kalman.statePre =  np.array([[6],[6]],np.float32)

for i in range(len(pos)):
    mes = np.reshape(pos[i,:],(2,1))

    x = kalman.correct(mes)

    y = kalman.predict()
    print (kalman.statePost[0],kalman.statePost[1])
    print (kalman.statePre[0],kalman.statePre[1])
    print ('measurement:\t',mes[0],mes[1])  
    print ('correct:\t',x[0],x[1])
    print ('predict:\t',y[0],y[1])     
    print ('='*30)  
 

3.0 Python實戰–加上2維速度的2維位置跟蹤（滑鼠跟蹤）

與上面一樣，首先要建立滑鼠運動的模型，至少有兩個狀態變數：滑鼠位置x,y，也可以是四個狀態變數：位置x,y和速度vx，vy。兩個測量變數：滑鼠位置x,y。由於滑鼠的運動是個隨機運動，並沒有一個精確複雜的數學模型。在粒子濾波博文中，也做過影象跟蹤，跟那裡程式類似的是，滑鼠的位置主要通過上一時刻的位置再疊加一個隨機噪聲來預測。

python程式如下

import cv2
import numpy as np
import matplotlib.pyplot as plt

frame = np.zeros((800,800,3),np.uint8)
last_mes = current_mes = np.array((2,1),np.float32)
last_pre = current_pre = np.array((2,1),np.float32)

def mousemove(event, x,y,s,p):
    global frame, current_mes, mes, last_mes, current_pre, last_pre
    last_pre = current_pre
    last_mes = current_mes
    current_mes = np.array([[np.float32(x)],[np.float32(y)]])

    kalman.correct(current_mes)
    current_pre = kalman.predict()

    lmx, lmy = last_mes[0],last_mes[1]
    lpx, lpy = last_pre[0],last_pre[1]
    cmx, cmy = current_mes[0],current_mes[1]    
    cpx, cpy = current_pre[0],current_pre[1]    
    cv2.line(frame, (lmx,lmy),(cmx,cmy),(0,200,0))
    cv2.line(frame, (lpx,lpy),(cpx,cpy),(0,0,200))


cv2.namedWindow("Kalman")
cv2.setMouseCallback("Kalman", mousemove)
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.003
kalman.measurementNoiseCov = np.array([[1,0],[0,1]], np.float32) * 1

while(True):
    cv2.imshow('Kalman',frame)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

cv2.destroyAllWindows()