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

上一篇文章里介绍了贝叶斯滤波的理论框架，知道了贝叶斯滤波假设了机器人的状态服从某个概率分布，并且知道了如何利用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) <script type="math/tex" id="MathJax-Element-23088">p({\bf{x}}_k|{\bf{x}}_{k-1})</script>；
2、观测模型的概率分布 p(yk|xk) <script type="math/tex" id="MathJax-Element-23089">p({\bf{y}}_k|{\bf{x}}_{k})</script>；
3、0时刻机器人状态的先验分布 p(x0) <script type="math/tex" id="MathJax-Element-23090">p({\bf{x}}_{0})</script>。

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

3.1 高斯分布

一维高斯分布：
一维随机变量 x <script type="math/tex" id="MathJax-Element-23091">x</script>服从高斯分布，表示为x∼N(μ,σ2)<script type="math/tex" id="MathJax-Element-23092">x \sim N(\mu,\sigma^2)</script>， μ <script type="math/tex" id="MathJax-Element-23093">\mu</script>代表随机变量的均值， σ2 <script type="math/tex" id="MathJax-Element-23094">\sigma^2</script>代表随机变量的方差。概率密度函数如下：

p(x)=12π−−√σexp(−(2−μ)22σ2)
<script type="math/tex; mode=display" id="MathJax-Element-23095">p(x)=\frac{1}{\sqrt{2\pi} \sigma}\exp{\left( {-\frac{(2-\mu)^2}{2\sigma^2}}\right)}</script>

多维高斯分布：
d维随机变量 x <script type="math/tex" id="MathJax-Element-23096">\bf{x}</script>服从高斯分布，表示为 x∼N(μ,Σ) <script type="math/tex" id="MathJax-Element-23097">{\bf{x}} \sim N( {\bf{\mu}},\bf{\Sigma})</script>， μ <script type="math/tex" id="MathJax-Element-23098">\bf{\mu}</script>代表随机变量的均值向量， Σ <script type="math/tex" id="MathJax-Element-23099">\bf{\Sigma}</script>代表随机变量的协方差矩阵。概率密度函数如下：

p(x)=1(2π)d/2|Σ|1/2exp(−12(x−μ)TΣ−1(x−μ))
<script type="math/tex; mode=display" id="MathJax-Element-23100">p({\bf{x}})=\frac{1}{(2\pi)^{d/2} |{\bf{\Sigma}}|^{1/2}}\exp{\left( {-\frac{1}{2} ({\bf{x}}-{\bf{\mu}})^T {\bf{\Sigma}^{-1}} ({\bf{x}}-{\bf{\mu}})} \right)}</script>

关于多元高斯分布的一点性质：
如果随机变量 x <script type="math/tex" id="MathJax-Element-23101">\bf{x}</script>和 y <script type="math/tex" id="MathJax-Element-23102">\bf{y}</script>具有联合高斯分布：

[xy]∼N([ab],[ACTCB])
<script type="math/tex; mode=display" id="MathJax-Element-23103"> \left[ \begin{matrix} {\bf{x}} \\ {\bf{y}} \end{matrix} \right] \sim N \left( \left[ \begin{matrix} {\bf{a}} \\ {\bf{b}} \end{matrix} \right], \left[ \begin{matrix} {\bf{A}} & {\bf{C}}\\ {\bf{C}}^T & {\bf{B}} \end{matrix} \right] \right) </script>
那么随机变量 x <script type="math/tex" id="MathJax-Element-23104">\bf{x}</script>和 y <script type="math/tex" id="MathJax-Element-23105">\bf{y}</script>的边缘分布和条件分布如下：
p(x)=N(a,A)
<script type="math/tex; mode=display" id="MathJax-Element-23106">p({\bf{x}})=N({\bf{a}},{\bf{A}})</script>
p(y)=N(b,B)
<script type="math/tex; mode=display" id="MathJax-Element-23107">p({\bf{y}})=N({\bf{b}},{\bf{B}})</script>
p(x|y)=N(a+CB−1(y−b),A−CB−1BT)
<script type="math/tex; mode=display" id="MathJax-Element-23108">p({\bf{x}}|{\bf{y}})=N({\bf{a}}+{\bf{CB^{-1}}}({\bf{y-b}}),{\bf{A-CB^{-1}B^T}})</script>
p(y|x)=N(b+CTA−1(x−a),B−CTA−1C)
<script type="math/tex; mode=display" id="MathJax-Element-23109">p({\bf{y}}|{\bf{x}})=N({\bf{b}}+{\bf{C^TA^{-1}}}({\bf{x-a}}),{\bf{B-C^TA^{-1}C}})</script>

3.2 线性高斯模型

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

x0∼N(m0,P0)
<script type="math/tex; mode=display" id="MathJax-Element-23110">{\bf{x}}_0 \sim N({\bf{m}}_0,{\bf{P}}_0)</script>
p(x0)=N(m0,P0)
<script type="math/tex; mode=display" id="MathJax-Element-23111">p({\bf{x}}_0) = N({\bf{m}}_0,{\bf{P}}_0)</script>
动态模型： Ak <script type="math/tex" id="MathJax-Element-23112"> {\bf{A}}_k</script>是系统矩阵， qk <script type="math/tex" id="MathJax-Element-23113"> {\bf{q}}_k</script>是系统噪声。
xk=Ak−1xk−1+qk−1
<script type="math/tex; mode=display" id="MathJax-Element-23114">{\bf{x}}_k = {\bf{A}}_{k-1} {\bf{x}}_{k-1} + {\bf{q}}_{k-1}</script>
qk∼N(0,Qk)
<script type="math/tex; mode=display" id="MathJax-Element-23115">{\bf{q}}_k \sim N(0,{\bf{Q}}_k)</script>
xk∼N(Ak−1xk−1,Qk−1)
<script type="math/tex; mode=display" id="MathJax-Element-23116">{\bf{x}}_k \sim N({\bf{A}}_{k-1} {\bf{x}}_{k-1},{\bf{Q}}_{k-1})</script>
p(xk|xk−1)=N(Ak−1xk−1,Qk−1)
<script type="math/tex; mode=display" id="MathJax-Element-23117">p({\bf{x}}_k|{\bf{x}}_{k-1}) = N({\bf{A}}_{k-1} {\bf{x}}_{k-1},{\bf{Q}}_{k-1})</script>
观测模型： Hk <script type="math/tex" id="MathJax-Element-23118"> {\bf{H}}_k</script>是观测矩阵， qk <script type="math/tex" id="MathJax-Element-23119"> {\bf{q}}_k</script>是观测噪声。
yk=Hkxk+rk
<script type="math/tex; mode=display" id="MathJax-Element-23120">{\bf{y}}_k = {\bf{H}}_k {\bf{x}}_k + {\bf{r}}_k</script>
rk∼N(0,Rk)
<script type="math/tex; mode=display" id="MathJax-Element-23121">{\bf{r}}_k \sim N(0,{\bf{R}}_k)</script>
yk∼N(Hkxk,Rk)
<script type="math/tex; mode=display" id="MathJax-Element-23122">{\bf{y}}_k \sim N({\bf{H}}_k {\bf{x}}_k,{\bf{R}}_k)</script>
p(yk|xk)=N(Hkxk,Rk)
<script type="math/tex; mode=display" id="MathJax-Element-23123">p({\bf{y}}_k|{\bf{x}}_k) = N({\bf{H}}_k {\bf{x}}_k,{\bf{R}}_k)</script>

3.3 推导卡尔曼滤波

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

p(xk|y1:k−1)=∫p(xk|xk−1)p(xk−1|y1:k−1) dxk−1
<script type="math/tex; mode=display" id="MathJax-Element-23145">p({\bf{x}}_k|{\bf{y}}_{1:k-1})=\int{p({\bf{x}}_k|{\bf{x}}_{k-1})p({\bf{x}}_{k-1}|{\bf{y}}_{1:k-1})}\ d{\bf{x}}_{k-1}</script>
更新方程：
p(xk|y1:k)=p(yk|xk)p(xk|y1:k−1)∫p(yk|xk)p(xk|y1:k−1) dxk
<script type="math/tex; mode=display" id="MathJax-Element-23146">p({\bf{x}}_k|{\bf{y}}_{1:k})=\frac{p({\bf{y}}_k|{\bf{x}}_k)p({\bf{x}}_k|{\bf{y}}_{1:k-1})}{\int{p({\bf{y}}_k|{\bf{x}}_k)p({\bf{x}}_k|{\bf{y}}_{1:k-1})} \ d{\bf{x}}_k}</script>

由于假设了k=0时刻的先验分布是高斯分布，且服从线性高斯模型，根据BF方程，容易验证 p(xk|y1:k−1) <script type="math/tex" id="MathJax-Element-23147">p({\bf{x}}_k|{\bf{y}}_{1:k-1})</script>以及 p(xk|y1:k) <script type="math/tex" id="MathJax-Element-23148">p({\bf{x}}_k|{\bf{y}}_{1:k})</script>也是服从高斯分布的。所以，我们可以先假设k时刻的先验分布为：

p(xk−1|y1:k−1)=N(mk−1,Pk−1)
<script type="math/tex; mode=display" id="MathJax-Element-23149">p({\bf{x}}_{k-1}|{\bf{y}}_{1:k-1}) =N({\bf{m}}_{k-1},{\bf{P}}_{k-1})</script>
现在，我们的问题就变为了推导出 p(xk|y1:k) <script type="math/tex" id="MathJax-Element-23150">p({\bf{x}}_{k}|{\bf{y}}_{1:k})</script>的形式。

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)
<script type="math/tex; mode=display" id="MathJax-Element-23151"> \begin{align} p({\bf{x}}_k,{\bf{x}}_{k-1}|{\bf{y}}_{1:k-1}) &= p({\bf{x}}_k|{\bf{x}}_{k-1})p({\bf{x}}_{k-1}|{\bf{y}}_{1:k-1}) \\ &= N({\bf{A}}_{k-1} {\bf{x}}_{k-1},{\bf{Q}}_{k-1}) N({\bf{m}}_{k-1},{\bf{P}}_{k-1}) \end{align} </script>
把k-1时刻和k时刻的状态变量看作一个整体，即 x′=[xk,xk−1]T <script type="math/tex" id="MathJax-Element-23152">{\bf{x}}'=[{\bf{x}}_k,{\bf{x}}_{k-1}]^T</script>，则有：
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])
<script type="math/tex; mode=display" id="MathJax-Element-23153">p({\bf{x}}'|{\bf{y}}_{1:k-1}) =p \left( {\left[ {\begin{matrix} {\bf{x}}_{k} \\ {\bf{x}}_{k-1} \end{matrix}}\right]} \Bigg|{\bf{y}}_{1:k-1} \right) = N{\left( {\left[ {\begin{matrix} {\bf{A}}_{k-1}{\bf{m}}_{k-1} \\ {\bf{m}}_{k-1} \end{matrix}}\right]} , {\left[ {\begin{matrix} {\bf{A}}_{k-1}{\bf{P}}_{k-1}{\bf{A}}_{k-1}^T+{\bf{Q}}_{k-1} & {\bf{A}}_{k-1}{\bf{P}}_{k-1}\\ {\bf{P}}_{k-1}{\bf{A}}_{k-1}^T & {\bf{P}}_{k-1} \end{matrix}}\right]}\right)}</script>

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

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)
<script type="math/tex; mode=display" id="MathJax-Element-23154">\begin{align} p({\bf{x}}_k|{\bf{y}}_{1:k-1}) &=\int{p({\bf{x}}_k|{\bf{x}}_{k-1})p({\bf{x}}_{k-1}|{\bf{y}}_{1:k-1})}\ d{\bf{x}}_{k-1}\\ &= \int{p({\bf{x}}_k,{\bf{x}}_{k-1}|{\bf{y}}_{1:k-1})}\ d{\bf{x}}_{k-1} \\ &= \int{p({\bf{x}}'|{\bf{y}}_{1:k-1})}\ d{\bf{x}}_{k-1} \\ &= \int{p \left( {\left[ {\begin{matrix} {\bf{x}}_{k} \\ {\bf{x}}_{k-1} \end{matrix}}\right]} \Bigg|{\bf{y}}_{1:k-1} \right)}\ d{\bf{x}}_{k-1} \\ &= N( {\bf{A}}_{k-1}{\bf{m}}_{k-1} , {\bf{A}}_{k-1}{\bf{P}}_{k-1}{\bf{A}}_{k-1}^T+{\bf{Q}}_{k-1}) \\ &= N(\hat{\bf{m}}_{k},\hat{\bf{P}}_{k}) \end{align} </script>

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])
<script type="math/tex; mode=display" id="MathJax-Element-23155"> \begin{align} p({\bf{x}}_k,{\bf{y}}_{k}|{\bf{y}}_{1:k-1}) &= p({\bf{y}}_k|{\bf{x}}_{k})p({\bf{x}}_{k}|{\bf{y}}_{1:k-1}) \\ &= N({\bf{H}}_{k} {\bf{x}}_{k},{\bf{R}}_{k}) N(\hat{\bf{m}}_{k},\hat{\bf{P}}_{k}) \\ &= p \left( {\left[ {\begin{matrix} {\bf{x}}_{k} \\ {\bf{y}}_{k} \end{matrix}}\right]} \Bigg|{\bf{y}}_{1:k-1} \right) \\ &= N{\left( {\left[ {\begin{matrix} \hat{\bf{m}}_{k} \\ {\bf{H}}_{k} \hat{\bf{m}}_{k} \end{matrix}}\right]} , {\left[ {\begin{matrix} \hat{\bf{P}}_{k} & \hat{\bf{P}}_{k}{\bf{H}}_{k}^T\\ {\bf{H}}_{k}\hat{\bf{P}}_{k} & {\bf{H}}_{k} \hat{\bf{P}}_{k}{\bf{H}}_{k}^T+ {\bf{R}}_{k} \end{matrix}}\right]}\right)} \end{align} </script>
根据更新方程，进一步推导：
p(xk|y1:k)=p(xk|yk,y1:k−1)
<script type="math/tex; mode=display" id="MathJax-Element-23156">p({\bf{x}}_k|{\bf{y}}_{1:k})=p({\bf{x}}_k|{\bf{y}}_k,{\bf{y}}_{1:k-1})</script>
所以，我们已经知道 xk <script type="math/tex" id="MathJax-Element-23157">{\bf{x}}_k</script>与 yk <script type="math/tex" id="MathJax-Element-23158">{\bf{y}}_{k}</script>的联合概率分布，现在要求 yk <script type="math/tex" id="MathJax-Element-23159">{\bf{y}}_{k}</script>条件下 xk <script type="math/tex" id="MathJax-Element-23160">{\bf{x}}_k</script>的条件概率分布，可以利用3.1节介绍的高斯分布性质：
p(xk|y1:k)=N(mk,Pk)
<script type="math/tex; mode=display" id="MathJax-Element-23161">p({\bf{x}}_k|{\bf{y}}_{1:k})= N({\bf{m}}_{k},{\bf{P}}_{k})</script>
mk=m^k+P^kHTk(HkP^kHTk+Rk)−1[yk−Hkm^k]
<script type="math/tex; mode=display" id="MathJax-Element-23162">{\bf{m}}_{k}=\hat{\bf{m}}_{k} + \hat{\bf{P}}_{k}{\bf{H}}_{k}^T({\bf{H}}_{k} \hat{\bf{P}}_{k}{\bf{H}}_{k}^T+ {\bf{R}}_{k})^{-1}[ {\bf{y}}_{k}- {\bf{H}}_{k} \hat{\bf{m}}_{k}]</script>
Pk=P^k−P^kHTk(HkP^kHTk+Rk)−1HkP^k
<script type="math/tex; mode=display" id="MathJax-Element-23163">{\bf{P}}_{k}=\hat{\bf{P}}_{k} - \hat{\bf{P}}_{k}{\bf{H}}_{k}^T({\bf{H}}_{k} \hat{\bf{P}}_{k}{\bf{H}}_{k}^T+ {\bf{R}}_{k})^{-1}{\bf{H}}_{k} \hat{\bf{P}}_{k}</script>

令 Sk=HkP^kHTk+Rk <script type="math/tex" id="MathJax-Element-23164">{\bf{S}}_k={\bf{H}}_{k} \hat{\bf{P}}_{k}{\bf{H}}_{k}^T+ {\bf{R}}_{k}</script>， Kk=P^kHTkS−1k <script type="math/tex" id="MathJax-Element-23165">{\bf{K}}_k= \hat{\bf{P}}_{k}{\bf{H}}_{k}^T{\bf{S}}_k^{-1}</script>，可以将预测和更新两步综合如下：

m^k=Ak−1mk−1
<script type="math/tex; mode=display" id="MathJax-Element-23388">\hat{\bf{m}}_{k}={\bf{A}}_{k-1}{\bf{m}}_{k-1} </script>
P^k=Ak−1Pk−1ATk−1+Qk−1
<script type="math/tex; mode=display" id="MathJax-Element-23389">\hat{\bf{P}}_{k}= {\bf{A}}_{k-1}{\bf{P}}_{k-1}{\bf{A}}_{k-1}^T+{\bf{Q}}_{k-1}</script>
Sk=HkP^kHTk+Rk
<script type="math/tex; mode=display" id="MathJax-Element-23390">{\bf{S}}_k={\bf{H}}_{k} \hat{\bf{P}}_{k}{\bf{H}}_{k}^T+ {\bf{R}}_{k}</script>
Kk=P^kHTkS−1k
<script type="math/tex; mode=display" id="MathJax-Element-23391">{\bf{K}}_k= \hat{\bf{P}}_{k}{\bf{H}}_{k}^T{\bf{S}}_k^{-1}</script>
mk=m^k+Kk⋅[yk−Hkm^k]
<script type="math/tex; mode=display" id="MathJax-Element-23392">{\bf{m}}_{k}=\hat{\bf{m}}_{k} +{\bf{K}}_k \cdot [ {\bf{y}}_{k}- {\bf{H}}_{k} \hat{\bf{m}}_{k}]</script>
Pk=P^k−KkSkKTk
<script type="math/tex; mode=display" id="MathJax-Element-23393">{\bf{P}}_{k}=\hat{\bf{P}}_{k} - {\bf{K}}_k {\bf{S}}_k {\bf{K}}_k^T</script>

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

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

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

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

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

Logo
DAMO开发者矩阵

DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐

cover

TongSIM：智能机器仿真通用平台

avatar DAMO开发者矩阵

非结构化数据处理的容错机制设计

非结构化数据是指没有固定结构、无法用传统数据库（如SQL）直接存储和查询文本：用户评论、新闻 articles、社交媒体内容；图像：用户上传的照片、产品图片、医疗影像；音频/视频：语音留言、直播片段、短视频；其他：PDF、Word文档、日志文件。“不按常理出牌”——没有统一的 schema，格式千变万化，质量参差不齐。

avatar DAMO开发者矩阵

机器人诊断系统十年演进

摘要：机器人诊断系统十年演进（2015-2025）从救火式运维发展为Robot SRE闭环治理体系。核心演进包括：诊断对象从单机扩展到服务SLA，证据从经验升级为结构化数据链（metrics/logs/traces/replay），处置从人肉运维进化为自愈联动，治理从被动救火转变为防复发闭环。第三代系统通过五大模块（证据采集、事件编排、根因推理、自愈处置、防复发）实现"检测-定位-处置-

avatar DAMO开发者矩阵