opencv 卡尔曼 java_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)