引入机器人实际测量数据，基于贝叶斯网络表示这些数据的关系，基于这种表示，状态估计很容易进行，一般采用贝叶斯估计。

根据选取估计量的不同，又分为几种估计问题。

1.如果仅对机器人当前位姿状态$x_k$进行状态估计，这就是定位问题，对应的后验概率分布表述如式(7-88)所示。
$$
P(x_k|z_{1:k},u_{1:k},m)\tag {7-88}
$$
2.如果除了估计机器人的当前位姿，还同时对地图m进行估计，这就是在线SLAM问题，对应的后验概率分布表述如式7-89所示。
$$
P(x_k,m|z_{1:k},u_{1:k})\tag {7-89}
$$
3.如果要对机器人所有历史位姿$x_{1:k}$和地图m同时进行估计，这就是完全SLAM问题，对应的后验概率分布表述如式7-90所示。
$$
P(x_{1:k},m|z_{1:k},u_{1:k})\tag {7-90}
$$

定位问题和在线SLAM问题求解过程都是所谓的滤波方法。本节以式7-88举例，由于式7-88中的地图条件m是常量可以忽略，因此该式可以进一步简化为$P(x_k|z_{1:k},u_{1:k})$。本节通过对$P(x_k|z_{1:k},u_{1:k})$的分析，掌握滤波方法的基本原理。

​ 式7-90所示的完全SLAM问题也可以使用滤波方法求解，比如Fast-SLAM算法。但是，贝叶斯网络表示下的完全SLAM系统能很方便地转换为因子图表示，利用因子图表示完全SLAM问题，然后用最小二乘估计求解会更方便。

7.4.1 贝叶斯估计

$$
bel(x_k)=P(x_k|z_{1:k},u_{1:k})
$$
$bel(x_k)$也被称为置信度。

利用贝叶斯准则将$P(x_k|z_{1:k},u_{1:k})$进行分解，如式7-91，式中的分母$P(z_k|z_{1:k-1},u_{1:k})$可以直接计算，是一个常数值，因此可以忽略。为了保证$P(x_k|z_{1:k},u_{1:k})$是一个求和为1的概率分布，需要乘以归一化常数$\eta$保证其合法性。
$$
\begin{aligned}
bel(x_k)&=P(x_k|z_{1:k},u_{1:k})\
&=\frac{P(z_k|x_k,z_{1:k},u_k)P(x_k|z_{1:k},u_{1:k})}{P(z_k|z_{1:k},u_{1:k})}\
&=\eta\cdot P(z_k|x_k,z_{1:k-1},u_{1:k})P(x_k|z_{1:k-1},u_{1:k})
\end{aligned}\tag{7-91}
$$

贝叶斯网络中，有一条很重要的性质就是条件独立性。例如，除了直接指向某节点的原因节点外，其他所有节点与该节点都是条件独立的。利用条件独立性，可以得到式7-92的化简。
$$
P(z_k|x_k,z_{1:k-1},u_{1:k})=P(z_k|x_k)\tag{7-92}
$$
另外，可利用全概率公式$P(x)=\int P(x|y)P(y)dy$进行式7-93的化简。
$$
\overline{bel(x_k)}=P(x_k|z_{1:k-1},u_{1:k})=\int P(x_k|x_{k-1},z_{1:k-1},u_{1:k})P(x_{k-1}|z_{1:k-1},u_{1:k})dx_{k-1}\tag{7-93}
$$
利用条件独立性，同样也可以进行式7-94所示的简化。
$$
P(x_k|x_{k-1},z_{1:k-1},u_{1:k})=P(x_k|x_{k-1},u_k) \tag{7-94}
$$
同样，由于k时刻的控制量$u_k$并不会影响k-1时刻的状态$x_{k-1}$，所以可以进行如式7-95所示的化简。
$$
P(x_{k-1}|z_{1:k-1},u_{1:k})=P(x_{k-1}|z_{1:k-1},u_{1:k-1})\tag{7-95}
$$
将式7-95和式7-94带入到式7-93可得，化简结果如式7-96所示：
$$
\begin{aligned}
\overline{bel(x_k)}&=P(x_k|z_{1:k-1},u_{1:k})\
&=\int P(x_k|x_{k-1},z_{1:k-1},u_{1:k})P(x_{k-1}|z_{1:k-1},u_{1:k})dx_{k-1}\
&=\int P(x_k|x_{k-1},u_k)P(x_{k-1}|z_{1:k-1},u_{1:k-1})dx_{k-1}\
&=\int P(x_k|x_{k-1},u_k)\cdot bel(x_{k-1})dx_{k-1}
\end{aligned}\tag{7-96}
$$
将7-96与7-92带入到7-91中可得7-97，化简的结果就是后验概率分布$P(x_k|z_{1:k},u_{1:k})$的最终结果：
$$
\begin{aligned}
bel(x_k)&=P(x_k|z_{1:k},u_{1:k})\
&=\eta\cdot\ P(z_k|x_k)\cdot \overline{bel(x_k)}
\end{aligned}\tag{7-97}
$$
那么后验概率分布$P(x_k|z_{1:k},u_{1:k})$的计算过程可以整理成式(7-98)所示的形式。
$$
\begin{cases}
\overline{bel(x_k)}=\int P(x_k|x_{k-1},u_k)\cdot bel(x_{k-1})dx_{k-1}(运动预测)\
bel(x_k)=\eta\cdot P(z_k|x_k)\cdot\overline{bel(x_k)}(观测更新)
\end{cases}\tag{7-98}
$$
其中，$P(x_k|x_{k-1},u_k)$为运动模型的概率分布，$P(z_k|x_k)$为观测模型的概率分布，即$P(x_k|x_{k-1},u_k)$和$P(z_k|x_k)$由机器人测量数据给出。

​ 在已知状态初始值$x_0$的置信度后，利用运动数据$P(x_k|x_{k-1},u_k)$和前一时刻置信度$bel(x_{k-1})$预测出当前状态置信度$\overline{bel(x_k)}$，这个过程称为运动预测。

​ 因为运动预测存在较大误差，所以还需要利用观测数据$P(z_k|x_k)$对预测置信度$\overline{bel(x_k)}$进行修正，修正后的置信度为$bel(x_k)$，这个过程称为观测更新。

​ 显然，式7-98所示计算后验概率分布$P(x_k|z_{1:k},u_{1:k})$算法是一个递归过程，因此这个算法也称为递归贝叶斯滤波。

​ 后验概率分布$P(x_k|z_{1:k},u_{1:k})$没有确定的形式，即递归贝叶斯滤波是一种通用框架。给定不同形式的$P(x_k|z_{1:k},u_{1:k})$分布后，递归贝叶斯滤波也就对应不同形式的算法实现。按照高斯分布、非高斯分布、线性系统、非线性系统，递归贝叶斯分布可以划分为表7-2所示的4种情况。

实际问题中，使用概率密度函数$f(x_k,z_{1:k},u_{1:k})$来完整表示$P(x_k|z_{1:k},u_{1:k})$的非高斯分布情况，$f(x_k,z_{1:k},u_{1:k})$将是一个无限维空间中的函数，其在非线性系统中的运算复杂度非常高，计算代价将难以承受。因此，在非线性非高斯系统中必须进行近似计算以提高效率，尽管近似会带来精度的损失。

在非线性非高斯系统中，利用非参数方式表示概率分布，典型的实现算法就是直方图滤波(HF)和粒子滤波(PF)。

式7-98所示的递归贝叶斯框架只是给出了后验概率分布的计算方法，而状态估计是后验概率分布和估计策略结合的产物。即在讨论递归贝叶斯滤波的具体实现时，还需要讨论估计策略，以保证估计效果足够好。

7.4.2 参数化实现

​ 高斯分布可以用矩参数(均值和方差)进行表示，机器人中涉及的都是多维变量，所以这里讨论多维高斯分布，如式7-99。其中$x$是n维向量，均值$\mu$是n维向量，协方差矩阵$\Sigma$是$n \times n$的对称矩阵。式中$det(\Sigma)=|\Sigma|$，表示求矩阵$\Sigma$行列式的运算。
$$
P(x)=\frac{1}{\sqrt{(2\pi)^ndet(\Sigma)}}exp(-\frac{1}{2}(x-\mu)^T\Sigma^{-1}(x-\mu))\tag{7-99}
$$
高斯分布也可以用正则化参数表示，即**信息矩阵$\Omega$和信息向量$\xi$**。矩参数($\Sigma和\mu$)与正则参数（$\Omega和\xi$）存在下面的关系：
$$
\begin{cases}
\Omega=\Sigma^{-1}\
\xi=\Sigma^{-1}\mu
\end{cases}\tag{7-100}
$$

1. 卡尔曼滤波

​ 最简单的线性高斯系统，使用矩参数表示高斯分布，则式7-98所示的递归贝叶斯滤波的典型实现算法就是线性卡尔曼滤波。然后将线性卡尔曼滤波的应用范围扩展到非线性高斯系统，就成了扩展卡尔曼滤波和无迹卡尔曼滤波。

线性卡尔曼滤波

​ 线性高斯系统下，机器人的运动方程可以写成式7-102的形式，机器人的观测方程可以写成式7-103的形式。其中，$r_k$为运动过程中携带的高斯噪声，协方差矩阵记为$R_k$；$q_k$为观测过程携带的高斯噪声，协方差矩阵记为$Q_k$。
$$
x_k=A_kx_{k-1}+B_ku_k+r_k\tag{7-102}
$$

$$
z_k=C_kx_k+q_k\tag{7-103}
$$

那么依据式7-102和式7-103就可以写出运动模型的概率分布$P(x_k|x_{k-1},u_k)$和观测模型的概率分布$P(z_k|x_k)$的具体高斯分布形式，如式7-104和式7-105.利用机器人的实际测量数据，就能求出式7-104和式7-105的具体数值。
$$
P(x_k|x_{k-1},u_k)=\frac{1}{\sqrt{(2\pi)^ndet(R_k)}}exp(-\frac{1}{2}(x-(A_kx_{k-1}+B_ku_k))^TR_k^{-1}(x-A_kx_{k-1}+B_ku_k))\tag{7-104}
$$

$$
P(z_k|x_k)=\frac{1}{\sqrt{(2\pi)^ndet(Q_k)}}exp(-\frac{1}{2}(z_k-C_kx_k)^TQ_k^{-1}(z_k-C_kx_k))\tag{7-105}
$$

​ 卡尔曼滤波其实就是利用式7-107中的5个核心公式，递归计算状态$x_k$置信度的参数$\mu_k和\Sigma_k$。
$$
\begin{cases}
\left.\begin{matrix}
1.\overline{\mu_k}&=A_k\mu_{k-1}+B_k\mu_k\
2.\overline{\Sigma_k}&=A_k\Sigma_{k-1}A_k^T+R_k \end{matrix}\right}运动预测\
\left.\begin{matrix}3.K_k=\overline{\Sigma_k}C_k^T(C_k\overline{\Sigma_k}C_k^T+Q_k)^{-1}\end{matrix}\right}卡尔曼增益\
\left.\begin{matrix}4.\mu_k=\overline{\mu_k}+K_k(z_k-C_k\overline{\mu_k})\
5.\Sigma_k=(I-K_kC_k)\overline{\Sigma_k}\end{matrix}\right}观测更新\tag{7-107}
\end{cases}
$$
​ 将式7-98所示的递归贝叶斯滤波与式7-107所示的线性卡尔曼滤波对比，因为后验被假设为高斯分布，所以式7-98中的置信度运动预测$\overline{bel(x_k)}$就可以用式7-107中的高斯矩参数运动预测$\overline{\mu_k}$和$\overline{\Sigma_k}$替换。

​ 接着，式7-98中的置信度观测更新$bel(x_k)$就可以用式7-107中的高斯矩参数观测更新$\mu_k$和$\Sigma_k$替换。

​ 保证卡尔曼滤波具有好的估计效果的关键式7-107中的卡尔曼滤波增益$K_k$的取值。

​ 在线性高斯系统中，贝叶斯估计及其特例最小均方误差估计和最大后验估计都是等价的，估计策略的目标都是实现最优估计（也就是最小方差无偏估计）。

​ 利用贝叶斯估计、最小均方误差估计、最大后验概率估计的思路均可以推导出式7-107所示的卡尔曼滤波的5个核心公式。

扩展卡尔曼滤波

​ 式7-102和式7-103中假设机器人运动方程和观测方程都是线性的，这样高斯随机变量经过函数变换后仍然是一个高斯随机变量，保证线性卡尔曼滤波能进行闭式递归计算，这是卡尔曼滤波能够工作的重要原因。

​ 然而真实情况下，机器人的运动方程和观测方程都是非线性的，如式7-108和式7-109所示的函数都是非线性函数，即高斯随机变量经过非线性函数g和h变换后都将变成非高斯随机变量。
$$
x_k=g(x_{k-1},u_k)+r_k\tag{7-108}
$$

$$
z_k=h(x_k)+q_k\tag{7-109}
$$

​ 为了保证高斯随机变量的闭式递归计算，将非线性函数g和h进行线性化近似。

​ 在扩展卡尔曼滤波(EKF)中，采用一阶泰勒展开对非线性函数g和h进行线性化。

​ 如果g(x)函数在随机变量x的均值$\mu$处的切线$l(x)$（也就是一阶泰勒展开）近似替代g(x)，那么高斯分布P(x)经过切线$l(x)$线性变换后的分布$P(y)$还是高斯分布。

​ 扩展卡尔曼滤波的五个核心公式，可以写成式7-116。其实就是将式7-107中的2和5行公式中原来的线性变换矩阵$A_k、B_k和C_k$替换成了雅可比矩阵$G_k与H_k$。雅克比矩阵$G_k=g’(\mu_{k-1},u_k)$和$H_k=h’(\overline{\mu_k})$是实时计算的，每个时刻的取值都与当前均值相关。
$$
\begin{cases}
\left.\begin{matrix}
1.\overline{\mu_k}&=g(\mu_{k-1},u_k)\
2.\overline{\Sigma_k}&=G_k\Sigma_{k-1}G_k^T+R_k \end{matrix}\right}运动预测\
\left.\begin{matrix}3.K_k=\overline{\Sigma_k}H_k^T(H_k\overline{\Sigma_k}H_k^T+Q_k)^{-1}\end{matrix}\right}卡尔曼增益\
\left.\begin{matrix}4.\mu_k=\overline{\mu_k}+K_k(z_k-h(\overline{\mu_k})\
5.\Sigma_k=(I-K_kH_k)\overline{\Sigma_k}\end{matrix}\right}观测更新\tag{7-107}
\end{cases}
$$

无迹卡尔曼滤波

​ 扩展卡尔曼滤波中的非线性函数经过一阶泰勒展开后进行线性近似化，$P(x)$经过一阶泰勒展开的变换后，得到的高斯分布的均值与真实后验分布$P(y)$的均值存在偏差。如果x的不确定性和函数g的非线性度都较小，那么扩展卡尔曼滤波中非线性近似效果是好的。

但是当x的不确定性和函数g的非线性度都比较大时，需要使用无迹卡尔曼滤波来线性化近似效果。

​ 无迹卡尔曼滤波中采用的线性化技术叫做无迹变换。如图7-28，无迹变换选取高斯分布$P(x)$上的$\sigma$点，然后将这些$\sigma$点经过$g(x)$变换，利用变换后的点推算出变换后的高斯分布均值和协方差矩阵。

2.信息滤波

​ 同样考虑最简单的线性高斯系统，并用正则参数表示高斯分布，那么式7-97所示的递归贝叶斯滤波的典型实现算法就是线性信息滤波。扩展到非线性高斯系统，就是扩展信息滤波。

（1）线性信息滤波

高斯分布即可以用矩参数来表示，也可以使用正则参数来表示，本质上都是一样的，因此线性信息滤波的推导与线性卡尔曼滤波是类似的。线性信息滤波的核心公示，如式7-117。

将式7-107与7-117对比，可以发现线性卡尔曼滤波和线性信息滤波的性能是对偶的。线性卡尔曼滤波中的运动预测是增量的，线性信息滤波中的运动预测需要矩阵逆运算，因此线性卡尔曼滤波在运动预测上效率更高。

线性卡尔曼滤波的观测更新需要矩阵逆运算，而线性信息滤波中的观测更新是增量的，故线性信息滤波在观测更新上计算效率更高。

（2）扩展信息滤波

针对非线性高斯系统，需要进行线性近似，这就是扩展信息滤波。

7.4.3 非参数化实现

​ 卡尔曼滤波或者信息滤波中，需要用矩参数或正则参数对高斯分布进行参数化表示，然后对这些参数进行闭式递归计算。

​ 然而当分布不是高斯分布这种特殊形式时，就需要一个无限维概率密度函数$f(x_k,z_{1:k},u_{1:k})$描述，这种参数化显然不现实。因此需要使用非参数化的方法来表示这种非高斯分布的情况，即粒子滤波和直方图滤波。

1.直方图滤波

​ 式7-98所示的递归贝叶斯滤波的计算难点，因为状态量$x_k$值域为连续空间，所以式7-98中的积分运算将难以计算。如果状态量$x_k$值域为离散空间，那么式7-98中的积分就转变为求和运算，这样就容易计算了：式7-98就可以写成式7-119所示的离散递归贝叶斯滤波形式。

​ 其中$x_k^i$表示状态量$x_k$在离散空间中的一个取值点，上标i表示离散空间中点的索引号。将上一时刻状态量$x_{k-1}$的所有取值点（即遍历上标j）转移到$x_k^i$的概率求和，就得到了状态量$x_k$出现在取值点$x_k^i$的置信度预测，然后利用$x_k^i$处的观测信息对该预测置信度进行更新，就得到了状态量$x_k$出现在取值点$x_k^i$的置信度。

​ 式7-119只求出了状态量$x_k$在离散空间单个点上的置信度，需要遍历$x_k^i$上标i，重复利用式7-119求出状态量$x_k$在离散空间每个点上的置信度。那么，只需要将连续空间上的状态量$x_k$近似转换到离散空间上，就能利用式7-119计算状态量$x_k$的置信度了。

​ 直方图是一种将连续空间域转换为离散空间域的近似方法，如图7-29所示。原本的连续随机变量x的分布为$P(x)$，将x的值域划分成一个个区域，同一个区域内的所有概率密度用该区域平均概率密度替代，这样原来的连续型分布$P(x)$就替代成直方图表示了。

​ 由于讨论的是递归过程，因此这里假设k时刻的状态量$x_k$的值域被划分为若干个区域，每个区域用$x_k^i$表示，上标i表示区域的索引号。那么区域$x_k^i$可以由质心（表示区域$x_k^i$的概率加权中心坐标位置点，如式7-120）和平均概率表示。
$$
c(x_k^i)=\frac{\int_{x_k\in x_k^i}x_k\cdot P(x)dx_k}{\int_{x_k\in x_k^i}P(x_k)dx_k}\tag{7-120}
$$
因此，将运动模型的概率分布$P(x_k|x_{k-1},\mu_k)$和观测模型的概率分布$P(z_k|x_k)$也用区域$x_k^i$划分方式来转换为离散空间的形式。
$$
P(x_k^i|x_{k-1}^j,\mu_k)\approx \eta_1\cdot P(c(x_k^i)|c(x_{k-1}^j),\mu_k)\tag{7-121}
$$

$$
P(z_k|x_k^i)\approx \eta_2 \cdot P（z_k|c(x_k^i)）\tag{7-122}
$$

2.粒子滤波

​ 粒子是一种近似表示后验概率分布的非参数化方法，即粒子。粒子滤波用一系列通过后验概率分布随机采样的状态粒子近似表示后验概率分布，采样得到的状态粒子点的疏密程度与该区域后验概率分布大小成正比，也就是说状态粒子点的疏密程度间接反映了后验概率分布的大小。这样粒子点就可以直接参与系统的非线性变换，并且利用运动和观测进行重新采样以调整状态粒子点的疏密程度。

​ 粒子滤波是一种基于遗传进化的算法，粒子经过运动和观测过程的筛选后，粒子点会逐渐集中到后验概率高的区域。

递归贝叶斯滤波总结