SLAM学习笔记——贝叶斯滤波（高斯滤波篇）

参考概率机器人，只有如何使用
先明确几个定义：

• Z代表观测状态，比如雷达、GPS告诉我们机器人在哪里
• u代表控制数据，比如向前十米、向后十米，这个数据可以由里程计、IMU提供
• x代表真实的状态
• 执行控制后的后验概率
  
• 结合观测后的后验概率
  

贝叶斯滤波

• 我们把积分当成求和理解更清晰，比如上一次预测出来我们的位置在x1,x2，他们的概率分别为0.1，0.9，经过前进十米控制指令后他们自然会预测在x1+10，x2+10位置处，但是你的里程计不一定准，x1运动十米后可能在x1+9,x1+10,x1+11处，概率分别为0.1,0.8,0.1。然后乘一下，x1+9概率为0.1x0.1，x1+10为0.1x0.8，x1+11概率为0.1x0.1。x2也是同理，就算出来了。如果概率分布是连续的就要积分。
  其实就是全概率公式（注意不是贝叶斯，不要脑残！）
  
• 然后结合观测数据，和上面本质是一样的。你不可能通过你的位置预测出你可能看到所有情况的概率。因此求和结果不是1，需要归一化。
  
  结合这张图看看你有没有理解这个过程
  

高斯滤波

把贝叶斯滤波中的概率分布都当成高斯分布去处理

$\Sigma$是协方差矩阵，表达各个变量之间的关联程度。如果是一维的，那$\Sigma ={\sigma }^2$（$\sigma$就是小写的$\Sigma$）。如果所有状态是独立的，那么$\Sigma$只有对角线上有值，代表每个变量的方差。

卡尔曼滤波（KF）

高斯滤波器的第一种实现方式，可以解决连续状态的线性高斯系统

• 状态转移方程
  
  比如原来位于(x,y,z)，沿着x方向运动十米，噪声为±1m。AB都是单位矩阵，ut就是(10,0,0)，$\epsilon$代表噪声(±1，±1，±1)，那么$\overline{bel}$就可以算出来了。当然这里是高斯分布。后面会省略掉噪声，因为噪声不可控，如果是系统性偏差就想办法修正，如果是随机噪声只能放他去了。

• 测量概率
  
  噪声均值为0，方差为Q

直接看伪代码，推导感兴趣自己学习

$\mu$代表平均值，就是各个状态的平均值，就是x的平均值。$\Sigma$是上一个状态的协方差矩阵，ut是控制状态转移，zt是观测结果，C是观测数据到状态的转移矩阵，Q是观测模型的协方差

• 第二行通过状态转移方程计算出p(x|u)的平均值，也就是$\overline{bel}$的平均值
• 第三行是协方差矩阵的更新方式，可以算出$\overline{bel}$的协方差矩阵
• 第四行算出的是卡尔曼增益，这个值越大，代表越相信观测数据，当卡尔曼增益等于0的时候，则完全不相信观测数据。显然，观测越不准确，Q越大，K越小，修正效果越小。
• 第五、第六行就是计算$bel$

拓展卡尔曼滤波（EKF）

卡尔曼滤波无法在非线性的转移方程下使用，拓展卡尔曼滤波就是将转移方程做一阶泰勒展开，线性近似推导而出的

• g代表空置状态转移方程，h代表观测转移，G是g的雅可比矩阵，H是h的雅可比矩阵

密度滤波器（AKF）

使用矩估计进行线性化

无迹卡尔曼滤波（UKF）

带权采样，然后对采样点进行非线性变换，利用变换完的点再生成新的高斯分布

信息滤波&拓展信息滤波

卡尔曼的对偶，信息矩阵是协方差矩阵的逆，在迭代过程中计算更快，但我不知道怎么算出来的，写在这里只是保证完整性。