OpenCV 卡尔曼滤波

什么是卡尔曼滤波

假设我们有两个传感器，测的是同一个型号。可是他每次的读数都不太一样，怎么办呢？

取平均

再假设其中贵的那个传感器准确一些，便宜的那个差一些，有比取平均更好的办法吗

加权平均

怎么进行加权？假设两个传感器都是正态分布的，假设你知道这两个正态分布的方差，用着两个方差，你可以得到一个最优的权重。
接下来，重点来了：假设你只有一个传感器，但是你还有一个数学模型。模型可以帮你算出一个值，但也不是那么准。怎么办？

把模型算出来的值，和传感器测出的值，（就像两个传感器那样），取加权平均。

OK，最后一点说明：你的模型其实只是一个步长的，也就是说，知道x(k)，我可以求x(k+1)。问题是x(k)是多少呢？答案：x(k)就是你上一步卡尔曼滤波得到的、所谓加权平均之后的那个、对x在k时刻的最佳估计值。

于是迭代也有了。

这就是卡尔曼滤波。

总而言之，kalman滤波用在当测量值与模型预测值均不准确的情况下，用来计算预测真值的一种滤波方法。这在目标识别与追踪任务中经常用到。

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)