基于OpenCV的卡尔曼滤波的设计

本次我们介绍的是卡尔曼滤波，卡尔曼滤波算法在机器控制中应用十分广泛，当需要校准前后两个数据的时候，就需要卡尔曼滤波的登场了。也就是通过卡尔曼滤波来实现对下次数据的最佳预测，最佳估计。

总而言之，卡尔曼滤波是用在当测量值与模型预测值均不准确的情况下，用来计算预测真值的一种滤波方法。简而言之就是将卡尔曼滤波计算出来的值与当前的值进行加权平均。这在目标识别与追踪任务中经常用到。

如今，许多博客都有介绍卡尔曼滤波的简单应用-----，即实现对鼠标位置的跟踪。

首先，我们需要建立鼠标运动的模型，这就包括了两个状态变量：鼠标坐标x,y，以及两个需要测量，预测的变量：鼠标位置x,y。因为在我们使用的过程中，鼠标的运动是随机运动的，也就是并没有一个精确复杂的数学模型。因此，鼠标当前的位置主要是通过上一时刻的位置再叠加一个随机噪声来进行预测。

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()

在此算法中，主要应用到的知识是协方差。也就是通过修正协方差来估计新的位置，然后再根据当前的位置迭代修正协方差，进行下一次预估。

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

在这里，我们需要进行初始化。在这里，我们使用cv2.KalmanFilter(4,2)表示转移矩阵维度为4，测量矩阵维度为2

以上四个矩阵分别是：测量矩阵、转移矩阵、过程噪声协方差矩阵、测量噪声协方差矩

 

那么什么是协方差呢？

协方差表示的是两个变量的总体的误差，这与只表示一个变量误差的方差不同。 如果两个变量的变化趋势一致，也就是说如果其中一个大于自身的期望值，另外一个也大于自身的期望值，那么两个变量之间的协方差就是正值。 如果两个变量的变化趋势相反，即其中一个大于自身的期望值，另外一个却小于自身的期望值，那么两个变量之间的协方差就是负值。

当协方差绝对值越大，两者对彼此的影响越大，反之越小。噪声越大，预测结果越不稳定，越容易接近模型系统预测值，且单步变化越大，相反，若噪声小，则预测结果与上个计算结果相等。