机器人运动估计系列（番外篇）——从贝叶斯滤波到卡尔曼（上）

滤波理论在机器人状态估计里的重要性不言而喻，因此对滤波理论进行系统的了解非常有必要。本篇文章目的是从贝叶斯滤波（BF）的角度来串联现在流行的几种滤波方法：卡尔曼（KF）、扩展卡尔曼（EKF）、无迹卡尔曼（UKF）、粒子滤波（PF）等。

本篇文章主要参考文献为：
《贝叶斯滤波与平滑》，作者：希莫·萨日伽，译者：程建华等。
英文原版：《Bayesian Filtering and Smoothing》

为了简便起见，下文中用英文缩写来代表各种滤波器。
贝叶斯滤波：Bayes Filter：BF
卡尔曼滤波：Kalman Filter：KF
扩展卡尔曼：Extend Kalman Filter：EKF
无迹卡尔曼：Unscent Kalman Filter：UKF

1 为什么是BF？

首先，BF是什么？BF是依据机器人传感器获取的观测数据，利用Bayes公式（概率论）去估计机器人的状态的一种手段。这里的关键点在于，BF假设了机器人当前的状态服从某一个概率分布，将机器人状态估计问题建模为一个概率分布的估计问题，从而利用概率论的数学工具来解决机器人的状态估计。

那么为什么要用BF呢？这是因为，机器人的运动模型以及传感器的观测模型常常受到噪声干扰，而这种噪声都是随机性的。这样建模就可以有利于我们把噪声的分布、统计特性给估计出来，从而去除噪声，得到真实观测以及真实状态。

那么，BF具体是怎么对机器人状态估计问题建模的呢？

举一个例子，我做了一只小狗机器人，它身上装了GPS，可以时刻返回小狗机器人的位置。为了遥控它，我还给它装了一个遥控接收装置（根据小狗之前在的位置加上遥控量，也能算出小狗的位置）。但是，我遇到两个问题。一是，我花的钱太少了，买到GPS不太准，本来小狗应该在(5,3)的位置，但是测量的结果却是(5.1,2.8)。二是，小狗机器人接收到指令是前进1米，但是实际走的却可能是0.98米。小狗机器人的观测过程和运动过程中都存在噪声的干扰，这使得我想要知道我的小狗跑到哪里变得很困难，需要借助滤波手段来估计小狗位置。

基于这个例子，由于受到噪声污染，小狗机器人位置和GPS的观测可以被建模为概率分布，以表征其不确定性，即：

xk∼p(xk|x1:k−1,y1:k−1)

x_k \sim p(x_k|x_{1:k-1},y_{1:k-1})

yk∼p(yk|x1:k,y1:k−1)

y_k \sim p(y_k|x_{1:k},y_{1:k-1})
其中， x,yx,y分别代表小狗机器人的位置，以及GPS测量到的小狗机器人的位置。下标代表的是时间。这两个式子分别可以称为小狗机器人系统的动态模型（通常用状态方程描述，即从一个状态如何变化到另一个状态）和观测模型（通常用观测方程描述，即当前状态下能够获取到怎么样的观测），表达的意思是：k时刻小狗的位置服从以1到k-1时刻的小狗位置以及1到k-1时刻的GPS测量值为条件的一个概率分布；另外，k时刻GPS的测量值服从以1到k时刻的小狗位置以及1到k-1时刻的GPS测量值为条件的一个概率分布。

当然，上面这种建模方式是比较一般化的情况，而有些情况下（例如本例），k时刻的状态和观测未必和很久以前的状态与观测有关系，这时可以对这种建模方式进行一定简化。

简化一：状态量的马尔科夫性
假设k时刻小狗的位置只与k-1时刻的小狗的位置有关，由此：

xk∼p(xk|xk−1)

x_k \sim p(x_k|x_{k-1})
简化二：观测量的条件独立性
假设k时刻GPS的位置的值只与k时刻小狗的位置有关，由此：

yk∼p(yk|xk)

y_k \sim p(y_k|x_k)
上述两个简化的含义非常直观，通常的机器人状态估计问题基本能进行这样的简化。

当然，机器人状态估计不一定只能用求解概率分布的方法。最小二乘（观测与状态误差最小）也是常用的方法，卡尔曼滤波最早实际上是从最小二乘法的基础上推导出来的。

2 BF的基本框架

由于将机器人的状态估计问题考虑为了估计机器人状态的概率分布问题，那么就可以引入Bayes公式来进行计算了。这部分首先介绍Bayes公式，然后介绍利用Bayes公式估计状态概率分布的方法。

2.1 Bayes公式

这个公式是接下去一切讨论的前题，罗列如下，不多做解释，其中a、b、c是不同的随机变量。
Bayes公式：

p(a|b)=p(b|a)p(a)p(b)

p(a|b)=\frac{p(b|a)p(a)}{p(b)}
此外，由此公式延伸出的公式还有：

p(a,b)=p(a|b)p(b)=p(b|a)p(a)

p(a|b)=p(b|a)p(a)∫p(b|a)p(a) da

p(a|b)=\frac{p(b|a)p(a)}{\int{p(b|a)p(a)\ da}}

p(a|b)p(b|c)=p(a,b|c)

p(a|b)p(b|c)=p(a,b|c)
以上公式非常重要，在下文中会陆续用到。读者要是不理解可以先将其接受，之后再去查阅资料。

2.2 BF基本方程

首先，我们需要明确BF的目标是什么？
估计机器人状态的概率分布。在小狗的例子中，就是通过手头信息估计出k时刻小狗位置的概率分布p(xk|y1:k)p(x_k|y_{1:k})。而我们手头的信息就包括，1到k-1时刻小狗的位置x1:k−1x_{1:k-1}以及1到k时刻 GPS的测量值y1:ky_{1:k}。

2.2.1 批处理方法

因此，我们首先想到的方法大概是：

p(xk|y1:k)=p(y1:k|xk)p(xk)p(y1:k)

p(x_{k}|y_{1:k})=\frac{p(y_{1:k}|x_{k})p(x_{k})}{p(y_{1:k})}
这种方法也就是所谓的批处理贝叶斯方法。但是这种方法每次都要拿所有的测量值来重新计算概率分布，对于计算机是个沉重的负担。因此，有了改进的基于递归的贝叶斯滤波方法。

2.2.2 递归方法

事实上，在k时刻，我们除了GPS的测量值y1:ky_{1:k}以外，还有k-1时刻对小狗位置的估计p(xk−1|y1:k−1)p(x_{k-1}|y_{1:k-1})。（需要注意，当k=1时，由于k-1=0时刻没有观测值，小狗位置的概率分布p(x0)p(x_0)比较特殊，与观测无关。这个分布由人为给定，被当做小狗位置的先验分布。）

因此，我们根据k-1时刻的小狗位置分布和k时刻的GPS观测可以利用递归的方法得到k时刻的位置分布，这也就是BF的核心。估计k时刻小狗位置的计算过程包括两步，如下：

1-Step：预测
这一步是用p(xk−1|y1:k−1)p(x_{k-1}|y_{1:k-1})去得到p(xk|y1:k−1)p(x_{k}|y_{1:k-1})，注意这里并没有用到k时刻的观测值。

p(xk|y1:k−1)=∫p(xk|xk−1)p(xk−1|y1:k−1) dxk−1

p(x_k|y_{1:k-1})=\int{p(x_k|x_{k-1})p(x_{k-1}|y_{1:k-1})}\ dx_{k-1}
其中， p(xk|xk−1)p(x_k|x_{k-1})代表了状态由k-1时刻到k时刻的转移概率，也就是上文中介绍了简化了的小狗动态模型，这里的不确定性是由小狗运动过程中的噪声引入的。

这一步之所以称为预测步，可以理解为，在已知k-1时刻机器人位置的情况下，根据机器人本身运动学模型（通常是状态方程），去预测k时刻机器人的位置。p(xk−1|y1:k−1)p(x_{k-1}|y_{1:k-1})在这里被作为先验分布，p(xk|y1:k−1)p(x_{k}|y_{1:k-1})是由机器人运动学模型预测得到的预测分布。

2-Step：更新
这一步是用观测值yky_k去更新预测分布p(xk|y1:k−1)p(x_{k}|y_{1:k-1})，从而得到k时刻机器人位置的后验分布p(xk|y1:k)p(x_k|y_{1:k})。

p(x_k|y_{1:k})=\frac{p(y_k|x_k)p(x_k|y_{1:k-1})}{\int{p(y_k|x_k)p(x_k|y_{1:k-1})} \ dx_k}
其中， p(yk|xk)p(y_k|x_k)代表了依据k时刻状态 xkx_k观测到 yky_k的概率，也就是小狗机器人GPS的观测模型，这里的不确定性是由GPS观测过程中的噪声引入的。

这一步之所以称为更新是因为这步中用到了k时刻的观测量，从而将原本的预测分布p(xk|y1:k−1)p(x_{k}|y_{1:k-1})更新为了后验分布p(xk|y1:k)p(x_k|y_{1:k})。而这个后验分布，将作为估计k+1时刻小狗机器人位置的先验分布，从而开始下一轮的递归解算。

总结
BF的整个过程可以描述为，首先选定一个最初状态的先验分布p(x0)p(x_0)，进行预测步，得到k=1的预测分布，再用k=1的观测去更新预测分布得到k=1的后验分布，然后将这个后验分布作为先验分布，去预测k=2的预测分布，然后以此类推。。。

虽然BF的过程看着很简单，但是实际应用起来却并没有这么容易。首先，初始的先验分布不好猜测，动态模型以及观测模型的概率分布也不容易确定，只能去假设其形式，常见的比如高斯分布。另外，公式里的积分同样是个非常头疼的问题，只有再状态模型、观测模型很特殊的情况下才比较好求闭型解。因此，想要实际应用BF还需要做更多的假设，使得问题得到简化。而不同的假设方案，就由此引出了不同的滤波器。接下来，我就将进一步介绍KF、EKF、UKF是如何在BF的框架下被推导出来的！！！！！！请马上走开，不要回来！！！！！！