对Kalman Filter的理解

1. 卡尔曼滤波理论回顾

      对于一个动态系统，我们首先定义一组状态空间方程

     状态方程：     

     测量方程：      

        x_k是状态向量，z_k是测量向量，A_k是状态转移矩阵，u_k是控制向量，B_k是控制矩阵，w_k是系统误差（噪声），H_k是测量矩阵，v_k是测量误差（噪声）。w_k和v_k都是高斯噪声，即

                             

    整个卡尔曼滤波的过程就是个递推计算的过程，不断的“预测——更新——预测——更新……”

预测

     预测状态值：              

     预测最小均方误差：   

更新

    测量误差：                   

    测量协方差：                

    最优卡尔曼增益：         

    修正状态值：                

    修正最小均方误差：     

算法的核心思想是，根据当前的仪器"测量值" 和上一刻的 "预测量" 和 "误差",计算得到当前的最优量.   再 预测下一刻的量， 里面比较突出的是观点是. 把误差纳入计算, 而且分为预测误差和测量误差两种.通称为 噪声。还有一个非常大的特点是,误差独立存在, 始终不受测量数据的影响。

2. 例子

上面的ppt有助于入门理解。

但是在编程的时候你会发现,解释里面的数值23 没有很明确的指出，是指的那个时刻的23 是预测的23 还是上一课测量的23。

下面这段文字会有助于你更清晰的理解：

 

卡尔曼滤波是统计学的程序表达，要想深入理解，公式三 协方差的背后意义 需要学习统计学。

如果仅仅是使用的话，这5个公式套进程序里面还是很容易的。

看到这里如果你明白了原理，你再回过头看看，会发现，误差是独立存在的，误差不受数据的影响。误差按照统计学的协方差公式更新，跟数据无关。而且误差是不断变化的。

3. 通俗理解

    假设你有两个传感器，测的是同一个信号。可是它们每次的读数都不太一样，怎么办？取平均。再假设你知道其中贵的那个传感器应该准一些，便宜的那个应该差一些。那有比取平均更好的办法吗？加权平均。怎么加权？假设两个传感器的误差都符合正态分布，假设你知道这两个正态分布的方差，用这两个方差值，（此处省略若干数学公式），你可以得到一个“最优”的权重。接下来，重点来了：假设你只有一个传感器，但是你还有一个数学模型。模型可以帮你算出一个值，但也不是那么准。怎么办？把模型算出来的值，和传感器测出的值，（就像两个传感器那样），取加权平均。OK，最后一点说明：你的模型其实只是一个步长的，也就是说，知道x(k)，我可以求x(k+1)。问题是x(k)是多少呢？答案：x(k)就是你上一步卡尔曼滤波得到的、所谓加权平均之后的那个、对x在k时刻的最佳估计值。于是迭代也有了。这就是卡尔曼滤波。

    卡尔曼滤波（Kalman filter）是一种高效率的递归滤波器（自回归滤波器），它能够从一系列的不完全及包含噪声的测量中，估计动态系统的状态。卡尔曼滤波会根据各测量量在不同时间下的值，考虑各时间下的联合分布，再产生对未知变数的估计，因此会以只以单一测量量为基础的估计方式要准。

    卡尔曼滤波的算法是二步骤的程序。在估计步骤中，卡尔曼滤波会产生有关目前状态的估计，其中也包括不确定性。只要观察到下一个量测（其中一定含有某种程度的误差，包括随机噪声）。会通过加权平均来更新估计值，而确定性越高的量测加权比重也越高。算法是迭代的，可以在实时控制系统中执行，只需要目前的输入量测、以往的计算值以及其不确定性矩阵，不需要其他以往的资讯。

    使用卡尔曼滤波不用假设误差是正态分布^[3]，不过若所有的误差都是正态分布，卡尔曼滤波可以得到正确的条件机率估计。也发展了一些扩展或是广义的卡尔曼滤波，例如运作在非线性糸统的扩展卡尔曼滤波及无损卡尔曼滤波（unscented Kalman filter）。底层的模型类似隐马尔可夫模型，不过潜在变量的状态空间是连续的，而且所有潜在变量及可观测变数都是正态分布。

    卡尔曼滤波的一个典型实例是从一组有限的，包含噪声的，通过对物体位置的观察序列（可能有偏差）预测出物体的位置的坐标及速度。在很多工程应用（如雷达、计算机视觉）中都可以找到它的身影。同时，卡尔曼滤波也是控制理论以及控制系统工程中的一个重要课题。例如，对于雷达来说，人们感兴趣的是其能够跟踪目标。但目标的位置、速度、加速度的测量值往往在任何时候都有噪声。卡尔曼滤波利用目标的动态信息，设法去掉噪声的影响，得到一个关于目标位置的好的估计。这个估计可以是对当前目标位置的估计（滤波），也可以是对于将来位置的估计（预测），也可以是对过去位置的估计（插值或平滑）。