概率机器人（Probability Robotics）笔记 Chapter 3: 高斯滤波器（Gaussian Filters）

高斯滤波器

1. 简介

本章讲解一系列递归状态估计器，统称高斯滤波器。
高斯滤波器是最早的连续贝叶斯滤波器的计算量可实现版本。

高斯滤波器的思想是所有的置信度都是多元高斯。分布表达式：
$p(x)=\det (2\pi \Sigma {)}^{-frac12}\exp \{-\frac{1}{2}(x-\mu {)}^T{\Sigma }^{-1}(x-\mu )\}$
这个分布是由前两阶矩来描述的，即均值$\mu$与方差$\Sigma$。
$\Sigma$是一个二次、对称、半正定矩阵，维度是状态变量维度的平方。

高斯后验的后果是，高斯是单模型(unimodel)的，也就是有单个最大值；
因此只能用来做tracking，而不适用于全局定位之类的问题。

高斯有两种表达：矩表达和权威（canonical）表达，功能相同，但算法计算不同。

2. 卡尔曼滤波器

2.1 线性高斯系统

卡尔曼滤波器可用于线性系统中的滤波和预测，适用于连续状态，不适用于离散空间。
使用矩来表达后验：均值${\mu }_t$和方差${\Sigma }_t$。

需要满足三个条件，后验才是高斯分布：

1. 状态转移概率$p(x_t|u_t,x_{t-1})$是线性函数加高斯噪声（简称线性高斯），即
   $x_t=A_t{x}_{t-1}+B_t{u}_t+{ϵ}_t$,
   其中$x_t,u_t$分别是维度为n和m的列向量，$A_t,B_t$为系数阵，
   ${ϵ}_t$是零均值高斯分布的随机向量，方差记为$R_t$。
   后验高斯的均值为$A_t{x}_{t-1}+B_t{u}_t$，方差为$R_t$。
2. 测量概率$p(z_t|x_t)$是线性高斯，即$z_t=C_t{x}_t+{\delta }_t$；
   其中${\delta }_t$是零均值高斯分布的随机噪声向量，方差为$Q_t$。
   后验高斯的均值为$C_t{x}_t$，方差为$Q_t$。
3. 置信概率的初始分布需要是高斯，即均值为${\mu }_0$，方差为${\Sigma }_0$。

2.2 卡尔曼滤波器算法

算法 Kalman_filter(${\mu }_{t-1},{\Sigma }_{t-1},u_t,z_t$):
02····${\hat{\mu }}_t=A_t{\mu }_{t-1}+B_t{u}_t$
03····${\hat{\Sigma }}_t=A_t\Sigma t-1A_t^T+R_t$
04····$K_t={\hat{\Sigma }}_t{C}_t^T(C_t\widehat{{\Sigma }_t}{C}_t^T+Q_t{)}^{-1}$
05····${\mu }_t={\hat{\mu }}_t+K_t(z_t-C_t{\hat{\mu }}_t)$
06····${\Sigma }_t=(I-K_t{C}_t){\hat{\Sigma }}_t$
07····return ${\mu }_t,{\Sigma }_t$

算法中，$K$是卡尔曼增益，决定了测量对新的状态估计的影响。

2.3 图示

略过…

2.4 卡尔曼滤波器的数学推导

具体不写了，有些小技巧：

如果一个置信度满足$bel(x_t)=\eta \exp \{-J_t\}$，而$J_t$是$x_t$的二次函数，
则置信度是高斯分布，$J_t$对$x_t$的极值点（一阶导为0处）就是均值，
曲率（二阶导）的逆矩阵就是方差。

证明分两个部分，即运动预测和测量更新，
都是把高斯分布带入贝叶斯滤波器的泛化的表达式，然后求均值和方差。

3. 扩展卡尔曼滤波器

卡尔曼滤波器假设状态转换与测量模型都是线性高斯，现实中很难是这样。
扩展卡尔曼滤波器克服了线性的假设。这里假设状态转换和测量函数都非线性，即
$x_t=g(u_t,x_{t-1})+{ϵ}_t$和$z_t=h(x_t)+{\delta }_t$。

扩展卡尔曼滤波器计算的是真实置信分布的高斯近似。

3.1 泰勒展开线性化

高斯分布经过非线性函数通常就不是高斯了；
线性化是找一个与这个非线性函数在高斯分布均值点相切的线性函数；
之后这个高斯分布经过这个线性函数映射，仍然是高斯。

$$\begin{array}{lll}g(u_t,x_{t-1})& \approx & g(u_t,{\mu }_{t-1})+g^{\prime }(u_t,{\mu }_{t-1})(x_{t-1}-{\mu }_{t-1})\\ & =& g(u_t,{\mu }_{t-1})+G_t(x_{t-1}-{\mu }_{t-1})\end{array}$$

雅可比矩阵$G_t$的维度是n×n。

将上式带到高斯分布中，可以得到高斯分布$p(x_t|u_t,x_{t-1})$的矩：
均值为$g(u_t,{\mu }_{t-1})+G_t(x_{t-1}-{\mu }_{t-1})$, 方差为$R_t$。

对测量模型的处理相同，即
$h(x_t)\approx h({\hat{\mu }}_t)+h^{\prime }({\hat{\mu }}_t)(x_t-{\hat{\mu }}_t)=h({\hat{\mu }}_t)+H_t(x_t-{\hat{\mu }}_t)$
得到对应的高斯分布的矩为：
均值为$h({\hat{\mu }}_t)+H_t(x_t-{\hat{\mu }}_t)$, 方差为$Q_t$。

3.2 EKF算法

算法 Extended_Kalman_filter(${\mu }_{t-1},{\Sigma }_{t-1},u_t,z_t$):
02····${\hat{\mu }}_t=g(u_t,\mu t-1)$
03····${\hat{\Sigma }}_t=G_t{\Sigma }_{t-1}{G}_t^T+R_t$
04····$K_t$=\hat{\Sigma}_tH_t^{T(H_t\hat{\Sigma}_tH_t}T+Q_t)^{-1}$
05····${\mu }_t={\hat{\mu }}_t+K_t(z_t-h({\hat{\mu }}_t))$
06····${\Sigma }_t=(I-K_t{H}_t){\hat{\Sigma }}_t$
07····return ${\mu }_t,{\Sigma }_t$

与KF不同的是，EKF使用了雅可比矩阵$G_t$和$H_t$来代替线性的系数矩阵$A_t,B_t,C_t$。

3.3 数学证明

与KF基本相同，略过。

3.4 实践考虑

EKF非常流行，因为简单而且计算效率高，时间复杂度$O(k^{2.8}+n^2)$;　　
其中k是测量向量的维度，n是状态向量的维度。

EKF使用高斯，是一个单模型的分布，也就是只有一个猜测，伴随一定不确定性。
一种常用的EKF扩展是使用混合高斯（多个高斯相加）来表示后验。

泰勒展开的精确度给予两方面：函数的非线性程度，与不确定性（高斯峰的宽度）。
还有其他线性化的方式，如unscented KF，是选一些探针点，然后将其线性近似；
再如矩匹配（moments matching），是通过观测后验分布的均值和方差。

4. 信息滤波器

未完待续

5. 总结

未完待续