机器人学 —— 机器人感知（Kalman Filter）

　　对于机器人感知任务而言，经常需要预判物体的运动，保证机器人在物体与自身接触之前进行规避。比如无人机与障碍物的碰撞，足球机器人判断足球的位置。预判的前提是对当前状态进行准确的估计，比如足球的速度，障碍物靠近的速度。一般认为，测量是存在误差的 —— 眼见未必为实。

1、物体的运动学模型

　　物体的运动学模型使用状态向量来表达。以2维空间的质点运动为例，物体的运动学模型可以表达为 x = [ px py vx vy ]' 。其中 px py 表示物体的位置，vx vy 表示物体的速度。如果能够准确估计物体当前状态x 就可以对其一段时间后的状态进行判断。然而当前状态的测量噪声会对x 的估计产生干扰。Kalman Filter 的作用就是去除这些干扰。

　　卡尔曼滤波器的原理是使用马尔科夫链来对随机变量进行推测。马尔科夫链是一种特殊的贝叶斯网络，所以卡尔曼滤波器实际上是一种贝叶斯滤波器。其Graph Model 如下所示：

　　

　　这里需要指出的是 X_t-1 实际是未知的，但是我们以其估计量来作为实际值的最优估计。对于给定PGM，我们使用高斯分布来对随机变量进行建模 —— 高斯分布的几大优点见上篇博客。在不考虑子节点（还没有发生），父节点又被认为是给定的情况下，当前节点的CPD可以认为是父节点与观测节点的Factor product. 如果我们使用高斯模型进行建模，则factor 如下：
　　

　　其中，A代表转移矩阵，给定 t 时刻的状态，t+1时刻则由转移矩阵确定。vm 则表示转移噪声 —— t+1 时刻与 t 时刻中，速度不是常量。 vo 表示观测噪声 —— 我们对测量的肯定程度。使用高斯模型建模后，我们可以得到p(x_t+1|x_t) 与p(z_t|x_t) 的分布。第一个定义了x_t+1|x_t 的 factor ，第二个定义了观测模型的factor.  于是，我们可以用贝叶斯公式求取 p(x_t|x_t-1, z_t-1 ) .

　　

　　这里需要指出的是，一旦给定了zt 的父节点 xt ， 那么zt 和 xt-1 是 d- Seperate 的。所以 P(zt|xt,xt-1) = P(zt|xt).

2、最大后验估计

　　

 

　　最大后验估计对象如上，按照上述公式即可实现卡尔曼滤波器。实现后的误差及预测结果如图：

　　

　　卡尔曼滤波器的设计技巧在于，如果物体不是匀加速运动（如足球的滚动），那么位置方差可以给的较小（认为我们的运动跟踪算法帧率足够高，且相机检测够准），但是！！！速度方差一定要给的够大，以保证能够模拟变加速状态！！！