滤波算法：经典卡尔曼滤波

这两天学习了一些卡尔曼滤波算法的相关知识。相比其它的滤波算法，卡尔曼滤波在对计算量需求非常之低，同时又能达到相当不错的滤波结果。

1. 算法原理

网上看到一篇文章http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/对卡尔曼滤波讲解的十分形象透彻，国内也有这篇文章的中文翻译版，链接：https://blog.csdn.net/u010720661/article/details/63253509，这里还是先简单的介绍一下。

卡尔曼滤波实质上就是基于观测值以及估计值二者的数据对真实值进行估计的过程。预测步骤如图1所示：

 

图1 卡尔曼滤波原理流程图

 

假设我们能够得到被测物体的位置和速度的测量值，在已知上一时刻的最优估计值以及它的协方差矩阵的条件下（初始值可以随意取，但协方差矩阵应为非0矩阵），则有，，即：

                                             (1)

而此时，

                   (2)

如果我们加入额外的控制量，比如加速度，此时，，则此时：