卡尔曼滤波器python略解

因为想做目标追踪，所以还是用最经典卡尔曼滤波和匈牙利算法进行预测估计

卡尔曼滤波

大名鼎鼎的卡尔曼滤波实际就是利用上一时刻目标的位置和这一时刻由传感器或者其他方法测量得到的目标位置估计实际目标的算法。

当前时刻测量得到的目标位置因为存在误差或者噪声也不能直接使用，而预测量由于当事人不是神棍不能预言所以也不能直接用，那怎么样才能最有效地利用这两个有偏差的结果得到最接近实际的结果呢？因此不同的滤波算法应运而生…

数学原理

前提：所有的位置变化都是线性的

卡尔曼滤波不要求信号和噪声都是平稳过程的假设条件。对于每个时刻的系统扰动和观测误差（即噪声），只要对它们的统计性质作某些适当的假定，通过对含有噪声的观测信号进行处理，就能在平均的意义上，求得误差为最小的真实信号的估计值。

形式描述

X(k)=A X(k-1)+B U(k)+W(k)
Z(k)=H X(k)+V(k)

上述等式看起来很像离散系统的状态空间模型
X(k)、X(k-1)代表k及k-1时刻系统的状态变量
A, B为系统参数
U(k)是k时刻对系统的控制量
Z(k)是k时刻的测量值
H是测量系统的参数
W(k)和V(k)分别表示过程和测量的噪声

以例理解：
狙击手狙击目标，瞄准镜里的是目标的当前位置，而开枪射击的必须是目标下一时刻的位置，所以第一个等式相当于我在上一时刻从瞄准镜看到了目标，并估计得到这一时刻它的位置。第二个等式相当于我的眼睛近视，当前时刻看到的位置也不准，存在噪声。

由上述例子可知，如果用线性的思想，我们可以在上一时刻先预测我们当前时刻的测量值，然后把两者相减就可以得到我们预测的误差，然后再把这个误差按照一定比例补充到我们对实际位置的预测上去就可以尽可能贴近真实值，虽然有些绕，但是仔细想想还是能想通的。于是便有了：

X’(k)=A X*(k-1)+B U(k)+W(k)
Z’(k)=H X’(k)+V(k)

X*(k) = X’(k) + Kn( Z(k) - Z’(k) )
X*为估计值， X‘，Z’为预测值， Z为测量值， Kn为我们选取的系数。

进而得到
X*(k) = X’(k) + Kn( Z(k) - ( H X’(k)+V(k) ) )

卡尔曼滤波器用协方差矩阵来求取Kn

估计值与真实值的协方差矩阵 P(k) = E [ (X(k) - X*(k) ] x [ (X(k) - X*(k)]^T]
预测值与真实值的协方差矩阵 P’(k) = E [ (X(k) - X’(k) ] x [ (X(k) - X’(k)]^T]
将之前预测值与估计值的关系带入求解可得Kn = P’(k) / ( P’(k) + R ) 其中R为噪声的协方差矩阵
而为了便于进行序列推到，所以引入P’(k) 的递推形式：
P’(k+1) = AP’(k)A^T + Q

所以卡尔曼滤波主要分为预测和更新两个阶段，交替进行

接下来先看OpenCV以及封装好的cv2.kalmanFilter来实现鼠标轨迹跟踪

导入包、创建800*800的空帧、初始化测量值和预测值

import cv2
import numpy as np

frame = np.zeros((800, 800, 3), np.uint8)
last_measurement = current_measurement = np.array((2,1), np.float32)
last_prediction = current_prediction = np.zeros((2,1), np.float320)

定义鼠标移动回调函数，用来绘制跟踪效果

def mousemove(event, x, y, s, p):
    global frame, current_measurement, measurements, last_measurement,current_prediction,last_prediction
    # 参数更新
    last_prediction = current_prediction
    last_measurement = current_measurement
    current_measurement = np.array([[np.float32(x)],[np.float32(y)]])
    # 将当前测量值传入滤波器进行修正
    kalman.correct(current_measurement)
    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), (255, 0, 0))
    cv2.line(frame, (lpx, lpy), (cpx, cpy), (255, 0, 255))

显示窗口

cv2.namedWindow("kalman_tracker")
cv2.setMouseCallback("kalman_tracker", 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.0

while True:
    cv2.imshow("kalman_tracker", frame)
    if cv2.waitKey(30) & 0xFF == 27:
        break

cv2.destroyAllWindows()