卡尔曼滤波(Kalman filter)算法

卡尔曼滤波思想

• 你可以在任何含有不确定信息的动态系统中使用卡尔曼滤波，对系统下一步的走向做出有根据的预测，即使伴随着各种干扰，卡尔曼滤波总是能指出真实发生的情况。
• 在连续变化的系统中使用卡尔曼滤波是非常理想的，它具有占用内存小的优点（除了前一个状态量外，不需要保留其它历史数据），并且速度很快，很适合应用于实时问题和嵌入式系统。
• 它是一种结合先验经验、测量更新的状态估计算法。

实例讲解：垂直上抛球体自由落体运动

该算法的实践场景：向上抛一个球体，记录球体高度的观测值，同时根据前一个时间节拍的观测值和预测值求出当前节拍的球体高度的预测值。最终画出高度的观测值和预测值的曲线。

首先设置场景的基本参数以及相对应的时间节拍区间：

 

接下来设置球的初始条件和高度的观测数据： 

 接下来计算高度的预测数据，先是初始化预测数据的初始值和方差，然后在迭代更新中不断地更新预测数据的值和方差：

 

 迭代更新中的预测数据的值和方差：

 

 我们必须在预测值（粉红色）和传感器测量值（绿色）之间找到最优解。

 

这个重叠部分的均值就是两个估计最可能的值，也就是给定的所有信息中的最优估计。

 

 绘制观测高度的数据轨迹(加上带上噪声的)以及预测数据轨迹：

 

你会发现刚开始时候预测和真实值之间有一些差距，但是后来随着迭代的进行，二者之间的轨迹越来越一致了，这就说明即使伴随着各种干扰，卡尔曼滤波总是能指出真实发生的情况。