为什么要卡曼滤波？
简单来说，因为噪声影响，测量不准确，应用卡曼滤波来尽量去除噪声影响。
应用场景：
当预估的模型可知，如恒定、线性或多项式等。
有固定的模型后，才会有状态转移方程，kalmanfilter应用才方便。
opencv中kalmanfilter保留的接口很简单，只有三个：
init， predict及correct。
init仅仅是初始化卡曼滤波的维数；
predict就能得到估计的结果；
correct要将测量结果作为输入，得到最优化的估计，并为下次predict做准备。
除此之外，kalmanfilter类中还有10个比较复杂的参数，它们与kalmanfilter的原理密切相关。
5个矩阵是基本参数：
Mat transitionMatrix; //!< state transition matrix (A)
Mat controlMatrix; //!< control matrix (B) (not used if there is no control)
Mat measurementMatrix; //!< measurement matrix (H)
Mat processNoiseCov; //!< process noise covariance matrix (Q)
Mat measurementNoiseCov;//!< measurement noise covariance matrix (R)
5个矩阵是kalmanfilter公式计算的结果
Mat statePre; //!< predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)
Mat statePost; //!< corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))
Mat errorCovPre; //!< priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)*/
Mat gain; //!< Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)
Mat errorCovPost; //!< posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)
在使用kalmanfilter之前，需要初始化5个基本参数。 重点关注
transitionMatrix
processNoiseCov：过程噪声
measurementNoiseCov测量噪声
而
measurementMatrix： 直接观察测量，一般为1
controlMatrix：没有时，为0
errorCovPost 需要初始化，当其为1时，比为0时，收敛快的多。由公式可以看出。
statePost初始化尽量接近下一个状态值， 如果收敛很快，这个不是太重要。
kalmanfilter示例可以参考，opencv samples下的kalman.cpp。
白色：真实位置
红色：测量位置
绿色：估计位置
青色：最优化位置
