目标跟踪(3)——卡尔曼滤波器

《OpenCV 3计算机视觉——Python语言实现（原书第2版）》
第八章目标跟踪
|______8.4卡尔曼滤波器
本节只是对书中代码进行详细解读

卡尔曼滤波器算法分为两个阶段：
预测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()

运行结果：

卡尔曼相关解读：

1.opencv中kalman点跟踪例子
2.学习OpenCV——Kalman滤波
在后面这篇博客中介绍了矩阵值设置原因：

stateNum=4;//状态数，包括（x，y，dx，dy）坐标及速度（每次移动的距离）
measureNum=2;//观测量，能看到的是坐标值，当然也可以自己计算速度