卡尔曼滤波器的原理

1.卡尔曼原理

中值滤波法是一种非线性平滑技术。

卡尔曼滤波是一种线性最优滤波器。

卡尔曼滤波是一个最优化自回归数据处理算法。

     从机器学习和数据挖掘的角度来说，滤波是一个理性智能体为了把握当前状态以便进行理性决策所采取的行动。比如，前两天我们没出门，但是我们可以从房间里观察路上的行人有没有打伞（观测状态）来估计前两天有没有下雨（真实状态）。基于这些情况，现在我们要来决策今天（是否会有雨以及）外出是否需要打伞，这个过程就是滤波。

卡尔曼的大致思想可以理解为，实际值和观测值之间有误差，测量值之间也有误差，误差都是高斯白噪声。通过一次次迭代，一遍遍更新预测值，得到最优值。

2.温度例子一

拿温度的例子来说，根据你的经验判断，这个房间的温度是恒定的，也就是下一分钟的温度等于现在这一分钟的温度（假设我们用一分钟来做时间单位）。假设你对你的经验不是100%的相信，可能会有上下偏差几度。我们把这些偏差看成是高斯白噪声（White Gaussian Noise），也就是这些偏差跟前后时间是没有关系的而且符合高斯分配（Gaussian Distribution）。另外，我们在房间里放一个温度计，但是这个温度计也不准确的，测量值会比实际值偏差。我们也把这些偏差看成是高斯白噪声。
   好了，现在对于某一分钟我们有两个有关于该房间的温度值：你根据经验的预测值（系统的预测值）和温度计的值（测量值）。下面我们要用这两个值结合他们各自的噪声来估算出房间的实际温度值。

公式推导

我们认为温度恒定，因此此例子中，U(k)=0。

X(k)表示k时刻的实际值，注意此实际值表示根据k-1时刻预测出的实际值。真实的温度我们无从得知。

Z(k)表示k时刻的测量值，可以理解为k时刻温度计的值

^表示预测值，^X(k+1|k)表示k时刻预测的k+1时刻的温度值

~表示偏差，~X(k+1|k)表示真实值X(k+1|k)和预测值^X(k+1|k)的偏差。~Z(k+1|k)表示当前测量值和预测测量值的偏差。

A B H为矩阵。

W(k)和V(k)表示预测和测量中的噪声。假设为高斯白噪声。分别为我们预测的噪声和温度计的噪声。

公式（5）中，预测测量值是通过H和先验预测值得到的。

得到预测测量值后，再对当前状态进行后验预测，设后验预测值为^Z(k+1|k+1),则后验预测值的偏差为（6）。

（7）则为了得到当前的真实值X(k+1)。

~X(k+1|k)虽然无从得知，但和~Z(k+1|k)却存在一定相关性，最后就是要找到K，

K的第i行第j列的元素表示：对于第i个待测的状态量来说，第j个测量仪器测到的偏差的可信度。某个测量值对应的可信度越高，滤波器越“相信”该测量值。

X(k+1)=AX(k)+BU(k)+W(k)..........................(1)
Z(k+1) =  H X(k+1）+ V(k+1) ...................(2)
~X(k+1|k)= X(k+1) -^X(k+1|k)   ..................(3)
~Z(k+1|k)= Z(k+1)-^Z(k+1|k) ...............(4)
^Z(k+1|k)=H ^X(k+1|k) ....................(5)
~X(k+1|k+1)= X(k+1)-^X(k+1|k+1)......................(6)     
X(k+1) =^X(k+1|k)+~X(k+1|k)........................(7)

~X(k+1|k)=K*~Z(k+1|k)..................(8)

公式推导

后验预测的状态值

^X(k+1|k+1)=^X(k+1|k)+K*~Z(k+1|k)

~X(k+1|k+1)= X(k+1)-^X(k+1|k+1)

           =[^X(k+1|k)+~X(k+1|k)]-[^X(k+1|k)+K*~Z(k+1|k)]

           =~X(k+1|k)-K*~Z(k+1|k)

~Z(k+1|k)=Z(k+1)-^Z(k+1|k)

          = [H X(k+1）+ V(k+1)]-H ^X(k+1|k)

          = H[^X(k+1|k)+~X(k+1|k)]+ V(k+1)-H ^X(k+1|k)

          =H~X(k+1|k)+ V(k+1)

3.车运动例子二

车的运动

预测小车t-1时刻的状态

卡尔曼滤波器是最佳的线性滤波器，既然是线性关系，就可以写成矩阵的形式

F表示状态转移矩阵，从上一个状态预测当前状态。

B表示控制矩阵，表示控制量U怎样作用于当前状态。

得到卡尔曼滤波器第一个公式：

上式中x顶上的hat表示为估计值（而非真实值）。等式左端部分的右上标“-”表示该状态是根据上一状态推测而来的，稍后我们还将对其进行修正以得到最优估计，彼时才可以将“-”去掉。

我们预测具有不确定性，用协方差来表示这种不确定性。

（2）

观测值Z，观测不一定完全准确，所以加上噪声V

这五个公式分成预测组和更新组。预测组总是根据前一个状态来估计当前状态。更新组则根据观测信息来对预测信息进行修正，以期达到最优估计之目的。

参考资料：http://v.youku.com/v_show/id_XNzQwMTMwOTQ0.html