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

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

上一篇文章里介绍了贝叶斯滤波的理论框架，知道了贝叶斯滤波假设了机器人的状态服从某个概率分布，并且知道了如何利用Bayes公式对其概率分布更新。然而，前面的内容仅仅是介绍了其完美的数学原理，实际计算起来却并不适用。在这篇文章中，就将介绍如何通过一系列假设去简化贝叶斯滤波的计算过程。我们将介绍卡尔曼滤波器、扩展卡尔曼滤波器以及无迹卡尔曼滤波器的由来。

《贝叶斯滤波与平滑》，作者：希莫·萨日伽，译者：程建华等。
英文原版：《Bayesian Filtering and Smoothing》

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

3 从BF到KF

想要进行BF的预测与更新，我们必须知道三个概率分布：
1、机器人动态模型的概率分布 p(xk|xk−1) ；
2、观测模型的概率分布 p(yk|xk) ；
3、0时刻机器人状态的先验分布 p(x0) 。

但是，通常这三个分布是很难得到的，因此也无法真正求解BF。除非我们对其做一些简化的假设。比如，当我们假设动态模型、观测模型都是线性高斯模型、先验分布服从高斯分布时，我们便可以通过BF的两个方程来估计机器人的状态了。而在这种假设下得到的BF迭代方程实际上与卡尔曼滤波方程组时完全类似的。

3.1 高斯分布

一维高斯分布：
一维随机变量 x 服从高斯分布，表示为 x∼N(μ,σ2) ， μ 代表随机变量的均值， σ2 代表随机变量的方差。概率密度函数如下：

p(x)=12π−−√σexp(−(2−μ)22σ2)

多维高斯分布：
d维随机变量 x 服从高斯分布，表示为 x∼N(μ,Σ) ， μ 代表随机变量的均值向量， Σ 代表随机变量的协方差矩阵。概率密度函数如下：

p(x)=1(2π)d/2|Σ|1/2exp(−12(x−μ)TΣ−1(x−μ))

关于多元高斯分布的一点性质：
如果随机变量 x 和 y 具有联合高斯分布：

[xy]∼N([ab],[ACTCB])

那么随机变量 x 和 y 的边缘分布和条件分布如下：

p(x)=N(a,A)

p(y)=N(b,B)

p(x|y)=N(a+CB−1(y−b),A−CB−1BT)

p(y|x)=N(b+CTA−1(x−a),B−CTA−1C)

3.2 线性高斯模型

这里考虑的是比较一般的离散时变系统。假设状态先验分布服从高斯分布，状态方程和观测方程都是线性方程，但是都附加有高斯白噪声。
先验分布：

x0∼N(m0,P0)

p(x0)=N(m0,P0)

动态模型： Ak 是系统矩阵， qk 是系统噪声。

xk=Ak−1xk−1+qk−1

qk∼N(0,Qk)

xk∼N(Ak−1xk−1,Qk−1)

p(xk|xk−1)=N(Ak−1xk−1,Qk−1)

观测模型： Hk 是观测矩阵， qk 是观测噪声。

yk=Hkxk+rk

rk∼N(0,Rk)

yk∼N(Hkxk,Rk)

p(yk|xk)=N(Hkxk,Rk)

3.3 推导卡尔曼滤波

首先，回顾BF的两个主要方程：
预测方程：

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

更新方程：

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

由于假设了k=0时刻的先验分布是高斯分布，且服从线性高斯模型，根据BF方程，容易验证 p(xk|y1:k−1) 以及 p(xk|y1:k) 也是服从高斯分布的。所以，我们可以先假设k时刻的先验分布为：

p(xk−1|y1:k−1)=N(mk−1,Pk−1)

现在，我们的问题就变为了推导出 p(xk|y1:k) 的形式。

1 预测步骤
首先，计算预测方程的积分符号内的部分：

p(xk,xk−1|y1:k−1)=p(xk|xk−1)p(xk−1|y1:k−1)=N(Ak−1xk−1,Qk−1)N(mk−1,Pk−1)

把k-1时刻和k时刻的状态变量看作一个整体，即 x′=[xk,xk−1]T ，则有：

p(x′|y1:k−1)=p([xkxk−1]∣∣∣y1:k−1)=N([Ak−1mk−1mk−1],[Ak−1Pk−1ATk−1+Qk−1Pk−1ATk−1Ak−1Pk−1Pk−1])

所以，进一步，依据预测方程：

p(xk|y1:k−1)=∫p(xk|xk−1)p(xk−1|y1:k−1) dxk−1=∫p(xk,xk−1|y1:k−1) dxk−1=∫p(x′|y1:k−1) dxk−1=∫p([xkxk−1]∣∣∣y1:k−1) dxk−1=N(Ak−1mk−1,Ak−1Pk−1ATk−1+Qk−1)=N(m^k,P^k)

2 更新步骤
同样，先计算更新方程的积分符号内的部分：

p(xk,yk|y1:k−1)=p(yk|xk)p(xk|y1:k−1)=N(Hkxk,Rk)N(m^k,P^k)=p([xkyk]∣∣∣y1:k−1)=N([m^kHkm^k],[P^kHkP^kP^kHTkHkP^kHTk+Rk])

根据更新方程，进一步推导：

p(xk|y1:k)=p(xk|yk,y1:k−1)

所以，我们已经知道 xk 与 yk 的联合概率分布，现在要求 yk 条件下 xk 的条件概率分布，可以利用3.1节介绍的高斯分布性质：

p(xk|y1:k)=N(mk,Pk)

mk=m^k+P^kHTk(HkP^kHTk+Rk)−1[yk−Hkm^k]

Pk=P^k−P^kHTk(HkP^kHTk+Rk)−1HkP^k

令 Sk=HkP^kHTk+Rk ， Kk=P^kHTkS−1k ，可以将预测和更新两步综合如下：

m^k=Ak−1mk−1

P^k=Ak−1Pk−1ATk−1+Qk−1

Sk=HkP^kHTk+Rk

Kk=P^kHTkS−1k

mk=m^k+Kk⋅[yk−Hkm^k]

Pk=P^k−KkSkKTk

以上方程组实际上就是卡尔曼滤波器的方程组。至此，我们就从BF的框架下，基于线性高斯模型的假设，完全推导出了卡尔曼滤波器。

在本文中，虽然的确通过BF的理论框架得到了KF方程组，但是最初的推导实际上是基于不同的原理。另外，卡尔曼本人的推导方式回避了线性高斯模型的假设条件，因此卡尔曼滤波器实际上是更为通用的最优线性无偏估计方法，不仅仅局限于线性高斯模型。

回过头来，既然我们得到了在线性高斯模型假设下BF的解法，那么这种假设条件真的贴近于实际应用嘛？

答案应该分两头说，高斯白噪声的假设在实际中常常很适用，但是线性模型这种理想情况在实际应用中却很难遇到，实际应用中机器人的运动模型往往都是非线性的，那么这时候要如何去利用BF求解呢？扩展卡尔曼滤波EKF以及无迹卡尔曼滤波UKF，就派上了用场。

下一篇文章中，将会顺着此思路，继续介绍扩展卡尔曼滤波以及无迹卡尔曼滤波是如何在非线性系统中使用的。