概率机器人 第二章 递归状态估计（贝叶斯滤波）

学完了高博的《SLAM十四讲》，基本了解了SLAM的框架结构，然后看了一些ORB-SLAM的代码，跑了几个模型，看了几篇论文，感觉还是有很多关于状态估计的问题不是很清楚，然后在知乎看到了关于slam推荐书籍，有一本状态估计和概率机器人，但是状态估计目前只有英文版，发现概率机器人有中文版，所以就买了一本概率机器人。虽然翻译质量可能不是太好，但是还是能读，有些地方还是要看英文才行！感谢翻译概率机器人的作者，节省了时间！最近一口气把整个概率机器人的第一部分看完了，自己做了一点笔记，还是贴出来吧！做笔记只是为了加深自己理解，可能有些地方理解不到位，欢迎各位批评指正！
整本书分为4部分，分别是基础知识、定位、地图构建和规划与控制。基础知识一共6章。第一章绪论就不说了，其余五章分别是递归状态估计（贝叶斯滤波）、高斯滤波、非参数滤波、机器人运动（状态方程）、机器人感知（观测方程）。接下来我会把这六章的笔记都贴出来，加深自己理解！
接下来就开始贴第二章的笔记吧！
本章主要介绍的是机器人中贝叶斯滤波算法的基本思想。本章为机器人及其环境的交互建立了一个耦合的动态系统模型，也就是建立了两个方程，观测方程和控制方程。机器人通过控制方程来影响环境，观测方程仅仅观测环境变化，而不会对其产生影响。
在概率机器人中，机器人和环境通过上述两个方程联系在一起，这两个方程也可以看做是两个分布，即状态转移分布和观测分布。状态转移分布描述了状态如何随时间的变化而变化的特征（控制方程）。观测分布则是描述测量状态的特征（由控制方程引起的状态改变）。因为这两个都是以概率形式来描述的，所以就形成了概率机器人的基石。
机器人置信度是对给定所有过去传感器测量和所有过去控制的环境状态的一个后验分布。贝叶斯滤波是计算机器人置信度的基本算法，它是一个递归的，t时刻的置信度由t-1时刻的置信度来计算。置信度反应的是机器人有关环境状态的内部信息，比如机器人位姿，一般而言机器人是不知道自己的位姿的，它必须通过外部观测数据来推测出自己的位姿，也就是从内部置信度识别出自己的真实状态。在概率机器人当中是通过条件概率分布来表示置信度。对于真实的状态置信度分布可以为每一个可能的假设分配一个概率。置信度分布是以已经获得的观测数据为条件的关于状态变量的一个后验概率。

在还没有进行t时时刻的测量之前，仅仅基于t-1时刻之前的测量和t时刻之前的控制，我们可以得到一个预测概率，即：

它是一个基于之前状态的后验，在综合时刻t的测量之前，预测了t时刻的状态。由该预测计算bel(x_t )被称为测量更新。
贝叶斯滤波有一个非常重要的假设，就是马尔科夫假设，它假设当前状态是所有过去状态的总结，也就是说，下一个状态仅仅由当前状态决定，与之前的状态都无关。
贝叶斯滤波算法
贝叶斯算法有两个基本步骤组成：
1. 计算预测，也就是根据上一个状态和当前的控制预测当前的状态。
2. 测量更新，根据当前的测量结果去修正上一步的预测结果。这一步主要是根据贝叶斯法则来实现的,也就是条件概率。

整个算法如下：

这就是贝叶斯滤波算法了，现在先打算把笔记做完，然后过段时间尽量把每个算法的代码都写一下！