机器人运动估计系列（番外篇）——从贝叶斯滤波到卡尔曼（下）
# 机器人运动估计系列（番外篇）——从贝叶斯滤波到卡尔曼（下）
前面两篇文章里首先介绍了贝叶斯滤波的理论框架，之后对机器人模型做了线性高斯假设，推出了卡尔曼滤波的迭代方程组。**在这篇文章中，就将进一步介绍当机器人模型为非线性时该如何使用贝叶斯滤波。**我们将介绍扩展卡尔曼滤波器以及无迹卡尔曼滤波器的由来。
《贝叶斯滤波与平滑》，作者：希莫·萨日伽，译者：程建华等。
英文原版：《Bayesian Filtering and Smoothing》
为了简便起见，下文中用英文缩写来代表各种滤波器。
贝叶斯滤波：Bayes Filter：BF
卡尔曼滤波：Kalman Filter：KF
扩展卡尔曼：Extend Kalman Filter：EKF
无迹卡尔曼：Unscent Kalman Filter：UKF
4 从BF到EKF
这一部分阐述的是在非线性模型下，如何用BF来滤波。
我们还是先回到老问题，要用BF滤波必须知道三个概率分布：
1、机器人动态模型的概率分布 p ( x k ∣ x k − 1 ) p({\bf{x}}_k|{\bf{x}}_{k-1}) p(xk∣xk−1)；
2、观测模型的概率分布 p ( y k ∣ x k ) p({\bf{y}}_k|{\bf{x}}_{k}) p(yk∣xk)；
3、0时刻机器人状态的先验分布 p ( x 0 ) p({\bf{x}}_{0}) p(x0)。
之前解释过了，高斯白噪声是很常用的，所以我们依然可以假设0时刻的机器人状态服从高斯分布。但是，在非线性模型的情况下怎么去求解机器人动态模型或是观测模型的概率分布呢？
4.1 非线性模型的定义
先验分布：
x 0 ∼ N ( m 0 , P 0 ) {\bf{x}}_0 \sim N({\bf{m}}_0,{\bf{P}}_0) x0∼N(m0,P0)
p ( x 0 ) = N ( m 0 , P 0 ) p({\bf{x}}_0) = N({\bf{m}}_0,{\bf{P}}_0) p(x0)=N(m0,P0)
动态模型： f ( ⋅ ) f(\cdot) f(⋅) 是非线性系统函数， q k {\bf{q}}_k qk是系统噪声。
x k = f ( x k − 1 ) + q k − 1 {\bf{x}}_k = f( {\bf{x}}_{k-1}) + {\bf{q}}_{k-1} xk=f(xk−1)+qk−1
q k ∼ N ( 0 , Q k ) {\bf{q}}_k \sim N(0,{\bf{Q}}_k) qk∼N(0,Qk)
观测模型： h ( ⋅ ) h(\cdot) h(⋅)是非线性观测函数， r k {\bf{r}}_k rk是观测噪声。
y k = h ( x k ) + r k {\bf{y}}_k = h( {\bf{x}}_{k}) + {\bf{r}}_{k} yk=h(xk)+rk
r k ∼ N ( 0 , R k ) {\bf{r}}_k \sim N(0,{\bf{R}}_k) rk∼N(0,Rk)
4.2 概率分布的高斯逼近
思考一个问题，我们假设有一随机变量 x \bf{x} x服从高斯分布，然而通过一个非线性函数 g ( ⋅ ) g(\cdot) g(⋅)后， x \bf{x} x变换为了 y \bf{y} y，此时如何去估计随机变量 y \bf{y} y的概率分布呢？即：
x ∼ N ( m , P ) {\bf{x}} \sim N({\bf{m}},{\bf{P}}) x∼N(m,P)
y = g ( x ) {\bf{y}}=g(\bf{x}) y=g(x)
y ∼ ? ? ? {\bf{y}} \sim \ ??? y∼ ???
首先，明确一点，此时随机变量 y \bf{y} y的分布通常已经不再是高斯分布了。虽然它的分布可能是任意的，但是我们仍然可以用高斯分布去近似它，近似的手段有很多，泰勒展开是一种，无迹变换也是一种。本小节中主要介绍如何通过泰勒展开的方法，去实现对随机变量 y \bf{y} y分布的高斯逼近。
令 x = m + δ x {\bf{x}}={\bf{m}}+\delta {\bf{x}} x=m+δx， m \bf{m} m是随机变量 x \bf{x} x的均值。假设非线性函数 g ( ⋅ ) g(\cdot) g(⋅)可微，那么根据泰勒级数展开公式，有：
y = g ( x ) = g ( m + δ x ) ≈ g ( m ) + G x ( m ) δ x + . . . {\bf{y}} = g({\bf{x}}) = g({\bf{m}}+\delta {\bf{x}}) \approx g({\bf{m}}) + G_x({\bf{m}}) \delta {\bf{x}} + ... y=g(x)=g(m+δx)≈g(m)+Gx(m)δx+...
G x ( m ) = ∂ g ( x ) ∂ x G_x({\bf{m}}) = \frac{\partial g({\bf{x}})}{\partial {\bf{x}}} Gx(m)=∂x∂g(x)
我们可以只取展开式的前两项来作为原函数 g ( ⋅ ) g(\cdot) g(⋅)的近似，此时：
y = g ( x ) ≈ g ( m ) + G x ( m ) δ x {\bf{y}} = g({\bf{x}}) \approx g({\bf{m}}) + G_x({\bf{m}}) \delta {\bf{x}} y=g(x)≈g(m)+Gx(m)δx
我们假设仍然用高斯分布去逼近随机变量 y \bf{y} y的分布，因此要想知道它的分布，我们还需要计算出 y \bf{y} y的均值向量以及协方差矩阵：
E [ y ] = E [ g ( x ) ] ≈ E [ g ( m ) + G x ( m ) δ x ] = g ( m ) + G x ( m ) E [ δ x ] = g ( m ) + G x ( m ) ⋅ 0 = g ( m ) \begin{aligned} E[{\bf{y}}] &= E[g({\bf{x}})] \\ &\approx E[g({\bf{m}}) + G_x({\bf{m}}) \delta {\bf{x}}] \\ &= g({\bf{m}}) + G_x({\bf{m}}) E[\delta {\bf{x}}] \\ &= g({\bf{m}}) + G_x({\bf{m}}) \cdot 0 \\ &= g({\bf{m}}) \end{aligned} E[y]=E[g(x)]≈E[g(m)+Gx(m)δx]=g(m)+Gx(m)E[δx]=g(m)+Gx(m)⋅0=g(m)
E [ ( y − E [ y ] ) ( y − E [ y ] ) T ] = E [ ( g ( x ) − E [ g ( x ) ] ) ( g ( x ) − E [ g ( x ) ] ) T ] ≈ E [ ( g ( x ) − g ( m ) ) ( g ( x ) − g ( m ) ) T ] ≈ E [ ( G x ( m ) δ x ) ( G x ( m ) δ x ) T ] = G x ( m ) E [ δ x δ x T ] G x T ( m ) = G x ( m ) P G x T ( m ) \begin{aligned} E[( {\bf{y}} - E[{\bf{y}}] )( {\bf{y}} - E[{\bf{y}}] )^T] &= E[( g({\bf{x}}) - E[g({\bf{x}})] )( g({\bf{x}}) - E[g({\bf{x}})] )^T] \\ &\approx E[( g({\bf{x}}) - g({\bf{m}}) )( g({\bf{x}}) - g({\bf{m}}) )^T] \\ &\approx E[( G_x({\bf{m}}) \delta {\bf{x}} )( G_x({\bf{m}}) \delta {\bf{x}} )^T] \\ &= G_x({\bf{m}}) E[\delta {\bf{x}} \delta {\bf{x}}^T] G_x^T({\bf{m}}) \\ &= G_x({\bf{m}}) {\bf{P}} G_x^T({\bf{m}}) \end{aligned} E[(y−E[y])(y−E[y])T]=E[(g(x)−E[g(x)])(g(x)−E[g(x)])T]≈E[(g(x)−g(m))(g(x)−g(m))T]≈E[(Gx(m)δx)(Gx(m)δx)T]=Gx(m)E[δxδxT]GxT(m)=Gx(m)PGxT(m)
所以用一阶泰勒展开近似非线性函数，再用高斯分布去逼近随机变量 y \bf{y} y的概率分布，得到的结果如下：
y ∼ N ( g ( m ) , G x ( m ) P G x T ( m ) ) {\bf{y}} \sim N(g({\bf{m}}) , G_x({\bf{m}}) {\bf{P}} G_x^T({\bf{m}})) y∼N(g(m),Gx(m)PGxT(m))
至此，我们掌握了当 y = g ( x ) {\bf{y}}=g(\bf{x}) y=g(x)时，如何推导随机变量 y \bf{y} y的概率分布。但是，4.1小节中的非线性模型在此基础上还添加了一个白噪声，也就是 y = g ( x ) + q {\bf{y}}=g(\bf{x}) + {\bf{q}} y=g(x)+q。这要怎么处理呢？
其实也很简单，此时的随机变量 y \bf{y} y服从下面分布：
y ∼ N ( g ( m ) , G x ( m ) P G x T ( m ) + Q ) {\bf{y}} \sim N(g({\bf{m}}) , G_x({\bf{m}}) {\bf{P}} G_x^T({\bf{m}}) + {\bf{Q}}) y∼N(g(m),Gx(m)PGxT(m)+Q)
q ∼ N ( 0 , Q ) {\bf{q}} \sim N(0,{\bf{Q}}) q∼N(0,Q)
另外，在KF的推导过程中，我们曾用到过 p ( x k , x k − 1 ∣ y 1 : k − 1 ) p({\bf{x}}_k,{\bf{x}}_{k-1}|{\bf{y}}_{1:k-1}) p(xk,xk−1∣y1:k−1)的大小，在EKF的推导中我们也仍然需要用到。求解这个概率分布，实际上对应就是求解这里的 p ( x , y ) p({\bf{x}},{\bf{y}}) p(x,y)。
令 y ˉ = [ x , y ] T \bar{\bf{y}} = [{\bf{x}},{\bf{y}}]^T yˉ=[x,y]T，则按照上文中一样的推导方式，用高斯分布近似它，可以计算得到：
E [ y ˉ ] = [ m g ( m ) ] E[\bar{\bf{y}}] = \left[ \begin{matrix} {\bf{m}} \\ g({\bf{m}}) \end{matrix}\right] E[yˉ]=[mg(m)]
C o v [ y ˉ ] = [ P P G x T ( m ) G x ( m ) P G x ( m ) P G x T ( m ) + Q ] Cov[\bar{\bf{y}}] = \left[ \begin{matrix} {\bf{P}} & {\bf{P}} G_x^T({\bf{m}})\\ G_x({\bf{m}}){\bf{P}} & G_x({\bf{m}}){\bf{P}}G_x^T({\bf{m}})+{\bf{Q}} \end{matrix}\right] Cov[yˉ]=[PGx(m)PPGxT(m)Gx(m)PGxT(m)+Q]
在这里介绍的只是利用泰勒级数一阶展开来近似原本的非线性函数，这主要是为了避免计算量过大。如果有更高的性能要求时，也可以保留二阶或者更高阶的项，去近似原函数。
4.3 EKF的导出
事实上，除了 p ( x k ∣ x k − 1 ) p({\bf{x}}_k|{\bf{x}}_{k-1}) p(xk∣xk−1)与 p ( y k ∣ x k ) p({\bf{y}}_k|{\bf{x}}_{k}) p(yk∣xk)的不同以外，EKF的推导与KF的推导是完全类似的。
参考KF的推导过程，我们可以发现推导过程中最主要的步骤是估计出 p ( x k , x k − 1 ∣ y 1 : k − 1 ) p({\bf{x}}_k,{\bf{x}}_{k-1}|{\bf{y}}_{1:k-1}) p(xk,xk−1∣y1:k−1)以及 p ( x k , y k ∣ y 1 : k − 1 ) p({\bf{x}}_k,{\bf{y}}_{k}|{\bf{y}}_{1:k-1}) p(xk,yk∣y1:k−1)这两个分布的大小，知道这两个分布以后基本上就能依据多元高斯分布的性质直接写出BF的预测和更新的迭代方程了。因此，这里只介绍如何用4.2小节的方法去估计出这两个分布，而后推导预测和更新的迭代方程就不再啰嗦了，完全可以由读者自行来写出。
因为：
x k = f ( x k − 1 ) + q k − 1 {\bf{x}}_k = f( {\bf{x}}_{k-1}) + {\bf{q}}_{k-1} xk=f(xk−1)+qk−1
y k = h ( x k ) + r k {\bf{y}}_k = h( {\bf{x}}_{k}) + {\bf{r}}_{k} yk=h(xk)+rk
且k时刻状态的先验分布为：
p ( x k − 1 ∣ y 1 : k − 1 ) = N ( m k − 1 , P k − 1 ) p({\bf{x}}_{k-1}|{\bf{y}}_{1:k-1}) =N({\bf{m}}_{k-1},{\bf{P}}_{k-1}) p(xk−1∣y1:k−1)=N(mk−1,Pk−1)
k时刻状态的预测分布为：
p ( x k ∣ y 1 : k − 1 ) = N ( m ^ k , P ^ k ) p({\bf{x}}_{k}|{\bf{y}}_{1:k-1}) =N(\hat{\bf{m}}_{k},\hat{\bf{P}}_{k}) p(xk∣y1:k−1)=N(m^k,P^k)
所以：
p ( x k ∣ x k − 1 ) ≈ N ( f ( m k − 1 ) , F x ( m k − 1 ) P k − 1 F x T ( m k − 1 ) + Q k − 1 ) p({\bf{x}}_k|{\bf{x}}_{k-1}) \approx N(f( {\bf{m}}_{k-1}) , F_x({\bf{m}}_{k-1}){\bf{P}}_{k-1}F_x^T({\bf{m}}_{k-1})+{\bf{Q}}_{k-1} ) p(xk∣xk−1)≈N(f(mk−1),Fx(mk−1)Pk−1FxT(mk−1)+Qk−1)
p ( x k , x k − 1 ∣ y 1 : k − 1 ) ≈ N ( [ m k − 1 f ( m k − 1 ) ] , [ P k − 1 P k − 1 F x T ( m k − 1 ) F x ( m k − 1 ) P k − 1 F x ( m k − 1 ) P k − 1 F x T ( m k − 1 ) + Q k − 1 ] ) p({\bf{x}}_k,{\bf{x}}_{k-1}|{\bf{y}}_{1:k-1}) \approx N \left( \left[ \begin{matrix} {\bf{m}}_{k-1} \\ f({\bf{m}}_{k-1}) \end{matrix}\right], \left[ \begin{matrix} {\bf{P}}_{k-1} & {\bf{P}}_{k-1} F_x^T({\bf{m}}_{k-1})\\ F_x({\bf{m}}_{k-1}){\bf{P}}_{k-1} & F_x({\bf{m}}_{k-1}){\bf{P}}_{k-1}F_x^T({\bf{m}}_{k-1})+{\bf{Q}}_{k-1} \end{matrix}\right] \right) p(xk,xk−1∣y1:k−1)≈N([mk−1f(mk−1)],[Pk−1Fx(mk−1)Pk−1Pk−1FxT(mk−1)Fx(mk−1)Pk−1FxT(mk−1)+Qk−1])
p ( y k ∣ x k ) ≈ N ( h ( m ^ k ) , H x ( m ^ k ) P ^ k H x T ( m ^ k ) + R k ) p({\bf{y}}_k|{\bf{x}}_{k}) \approx N(h( \hat{\bf{m}}_{k} ) , H_x(\hat{\bf{m}}_{k})\hat{\bf{P}}_{k}H_x^T(\hat{\bf{m}}_{k})+{\bf{R}}_{k} ) p(yk∣xk)≈N(h(m^k),Hx(m^k)P^kHxT(m^k)+Rk)
p ( x k , y k ∣ y 1 : k − 1 ) ≈ N ( [ m ^ k h ( m ^ k ) ] , [ P ^ k P ^ k H x T ( m ^ k ) H x ( m ^ k ) P ^ k H x ( m ^ k ) P ^ k H x T ( m ^ k ) + R k ] ) p({\bf{x}}_k,{\bf{y}}_{k}|{\bf{y}}_{1:k-1}) \approx N \left( \left[ \begin{matrix} \hat{\bf{m}}_{k} \\ h(\hat{\bf{m}}_{k}) \end{matrix}\right], \left[ \begin{matrix} \hat{\bf{P}}_{k} & \hat{\bf{P}}_{k} H_x^T(\hat{\bf{m}}_{k})\\ H_x(\hat{\bf{m}}_{k})\hat{\bf{P}}_{k} & H_x(\hat{\bf{m}}_{k})\hat{\bf{P}}_{k}H_x^T(\hat{\bf{m}}_{k})+{\bf{R}}_{k} \end{matrix}\right] \right) p(xk,yk∣y1:k−1)≈N([m^kh(m^k)],[P^kHx(m^k)P^kP^kHxT(m^k)Hx(m^k)P^kHxT(m^k)+Rk])
4.4 关于EKF的小结
相比于其他的非线性滤波方法，EKF的优点是非常的简单，因此在工程应用中非常常见。但是它的缺点也不少，首先，EKF是在局部（上一时刻的状态均值处）对模型进行了线性化，并且只取了低阶的导数项去近似，所以在强非线性条件下，EKF性能不够好。其次，我们假设了随机变量是服从高斯分布的，因此在噪声非高斯情况下EKF也不能适用。最后，EKF还要求动态模型和量测模型可微，也限制了其适用范围。
5 从BF到UKF
待施工
6 参数估计Vs状态估计 & 贝叶斯学习Vs贝叶斯滤波
待施工