Published at发布于:： | PV/UV: / | Reading阅读:： 3 min
前段时间做Tracking，了解到Kalman滤波，感觉很不错。wikipedia上内容有点多，整理过后发现只有五个核心公式。
原理
Kalman滤波器不断把协方差递归，估算出最优值，并且保留了上一时刻的协方差（Kalman增益），因此它的运行很快，可以随不同的时刻而改变他的值。 首先，我们要引入一个离散控制过程系统，该系统可以用一个线性随机微分方程来描述：
$$ \begin{equation} X(k)=AX(k-1)+BU(k)+W(k) \end{equation} $$
加上系统的测量值：
$$ \begin{equation} Z(k)=HX(k)+V(k) \end{equation} $$
上面式子中，$X(k)$是$k$时刻的系统状态，$U(k)$是$k$时刻对系统的控制量。$AB$是系统参数，对于多模型系统，他们为矩阵。$Z(k)$是$k$时刻的测量值，$H$是测量系统的参数，对于多测量系统，$H$为矩阵。
$W(k)$和$V(k)$分别表示过程和测量的噪声。他们被假设成高斯白噪声他们的协方差分别为$Q$，$R$（假设不随系统状态变化而变化）。 对于满足上面条件（线性随机微分系统，过程和测量都是高斯白噪声），Kalman滤波器是最优的信息处理器。下面则用其自身和自身的协方差来估计系统的最优化输出。 首先，要利用系统的过程模型，来预测下一状态的系统。假设现在的系统状态是k，根据系统的模型，可以基于系统上一状态而预测出现在状态：
$$ \begin{equation} X(k|k-1)=AX(k-1|k-1)+BU(k) \end{equation} $$
上式中，$X(k|k-1)$是利用上一状态预测的结果，$X(k-1|k-1)$是上一状态最优的结果，$U(k)$为现在状态的控制量，如果没有控制量，它可以为0。 至此，系统结果已经更新，但对于$X(k|k-1)$的协方差还未更新，用$Cov$表示协方差则：
$$ \begin{equation} Cov(k|k-1)=ACov(k-1|k-1)A^{T}+Q \end{equation} $$
上式中，$Cov(k|k-1)$是$X(k|k-1)$对应的协方差，$Cov(k-1|k-1)$是$X(k-1|k-1)$对应的协方差，$A^{T}$表示$A$的转置，$Q$是系统过程的协方差。上面两个式子为对系统的预测。 当有了现在状态的预测结果，我们再收集现在状态的预测值。结合预测值和测量值，我们可以得到现在状态$k$的最优化估计值$X(k|k)$：
$$ \begin{equation} X(k|k)=X(k|k-1)+K_{g}(k)(Z(k)-HX(k|k-1)) \end{equation} $$
其中，$K_{g}$为Kalman增益：
$$ \begin{equation} K_{g}=Cov(k|k-1)H^{T}(HCov(k|k-1)H^{T}+R(k))^{-1} \end{equation} $$
于此，已经得到了k状态下的最优估算值$X(k|k)$。但是为了要另Kalman滤波器不断的运行下去直到系统过程结束，我们还要更新k状态下$X(k|k)$的协方差：
$$ \begin{equation} Cov(k|k)=(E-K_{g}H)Cov(k|k-1) \end{equation} $$
其中，$E$为单位矩阵。当系统进入$k+1$状态时，$Cov(k|k)$就是第二个公式的，因此，可以自回归下去。
应用
Kalman滤波器作为一个很好的预测模型，可以根据现有情况来预测未来情况，在做Tracking中则变得很有用处。对于一个对象$S$的Tracking，$S$的pos在域中漂移，利用Tracking可以使得pos平滑稳定。 在OpenCV中提供了CvKalman结构，但是使用起来仍然需要建立模型，前几天我根据上述原理对整个过程进行了封装：
|
|
下面则是构造函数和cvKalmanPositionPrediction方法的具体实现：
|
|
使用方法：
|
|