Python-openCV, Kalman filter实战

1.0 什么是卡尔曼模型

理论推导过于复杂，看个知乎上的解释吧
假设你有两个传感器，测的是同一个信号。可是它们每次的读数都不太一样，怎么办？
取平均。
再假设你知道其中贵的那个传感器应该准一些，便宜的那个应该差一些。那有比取平均更好的办法吗？
加权平均。
怎么加权？假设两个传感器的误差都符合正态分布，假设你知道这两个正态分布的方差，用这两个方差值，（此处省略若干数学公式），你可以得到一个“最优”的权重。
接下来，重点来了：假设你只有一个传感器，但是你还有一个数学模型。模型可以帮你算出一个值，但也不是那么准。怎么办？
把模型算出来的值，和传感器测出的值，（就像两个传感器那样），取加权平均。
OK，最后一点说明：你的模型其实只是一个步长的，也就是说，知道x(k)，我可以求x(k+1)。问题是x(k)是多少呢？答案：x(k)就是你上一步卡尔曼滤波得到的、所谓加权平均之后的那个、对x在k时刻的最佳估计值。
于是迭代也有了。
这就是卡尔曼滤波。

作者：Kent Zeng
链接：https://www.zhihu.com/question/23971601/answer/26254459
来源：知乎
著作权归作者所有。商业转载请联系作者获得授权，非商业转载请注明出处。

总而言之，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()