amcl算法--之概率基础和粒子滤波

本文主要是介绍amcl算法--之概率基础和粒子滤波，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

机器人定位（mobile robot localization)：

机器人定位，就是相对外部的参考系来估计机器人坐标的问题。给定了环境地图，机器人需要参照传感器数据，定位自己在地图上的相对位置。比如，已知环境中有三个相同的门，机器人的任务就是要通过检测和运动找到自己在哪。

这种特定的定位问题为成为全局定位（global localizaiton)。在全局定位中，机器人被放置在已知的环境中的某处，然后从头开始确定自己的位置。

概率基础

边缘概率（又称先验概率）：某个事件发生的概率。边缘概率是这样得到的：在联合概率中，把最终结果中那些不需要的事件通过合并成它们的全概率，而消去它们（对离散随机变量用求和得全概率，对连续随机变量用积分得全概率），这称为边缘化（marginalization），比如A的边缘概率表示为P(A)，B的边缘概率表示为P(B)。

联合概率表示两个事件共同发生的概率。A与B的联合概率表示为P(A∩B)或者P(A,B)或者P(AB)。

条件概率（又称后验概率）

在事件B已经发生的条件下，求事件A发生的概率，称这种概率为事件B发生条件下事件A发生的条件概率，条件概率表示为P(A|B)，读作“在B条件下A的概率”,。

接着，考虑一个问题：P(A|B)是在B发生的情况下A发生的可能性。

首先，事件B发生之前，我们对事件A的发生有一个基本的概率判断，称为A的先验概率，用P(A)表示；
其次，事件B发生之后，我们对事件A的发生概率重新评估，称为A的后验概率，用P(A|B)表示；
类似的，事件A发生之前，我们对事件B的发生有一个基本的概率判断，称为B的先验概率，用P(B)表示；
同样，事件A发生之后，我们对事件B的发生概率重新评估，称为B的后验概率，用P(B|A)表示。

等于AB同时发生的概率/B发生的概率

$P(A|B)=\frac{P(AB) }{P(B)}$ （1）条件概率定义

根据条件概率的定义，在事件B发生的条件下事件A发生的概率是

P(A|B)=P(A∩B)/P(B)

同样地，在事件A发生的条件下事件B发生的概率

P(B|A)=P(A∩B)/P(A)

整理与合并上述两个方程式，便可以得到：

P(A|B)P(B)=P(A∩B)=P(B|A)P(A)

接着，上式两边同除以P(B)，若P(B)是非零的，我们便可以得到贝叶斯定理的公式表达式：

$P(A|B)=\frac{P(B|A)*P(A)}{P(B)}$

理解的一个关键点，区分出规律和现象，就是将A看成“规律”，B看成“现象”，那么贝叶斯公式看成：

全概率定理

从条件概率和概率测量公理得出的一个有趣事实经常被称为全概率定理（theorem of total probability):

离散情况

$p(x)=\sum_{y}^{\indent}p(x|y)p(y)$

连续情况

$p(x)=\int p(x|y)p(y)dy$

联合分布概率公式：

$P(A,B|C)={P(A|B,C) }{P(B|C)}$

证明：首先根据条件概率的计算式（1）,并且利用计算技巧(再分子分母同时乘以一个因式)，可得

$P(A,B|C)=\frac{P(A,B,C) }{P(C)} =\frac{P(A,B,C) P(B,C)}{P(B,C)P(C)}=P(A|B,C)P(B|C)$

贝叶斯推断的含义

我们把P(A)称为"先验概率"（Prior probability），即在B事件发生之前，我们对A事件概率的一个判断。P(A|B)称为"后验概率"（Posterior probability），即在B事件发生之后，我们对A事件概率的重新评估。P(B|A)/P(B)称为"可能性函数"（Likelyhood），这是一个调整因子，使得预估概率更接近真实概率。

所以，条件概率可以理解成下面的式子：

后验概率　＝　先验概率ｘ 调整因子

这就是贝叶斯推断的含义。我们先预估一个"先验概率"，然后加入实验结果，看这个实验到底是增强还是削弱了"先验概率"，由此得到更接近事实的"后验概率"。

贝叶斯定理应用示例:

已知某种疾病的发病率是0.001，即1000人中会有1个人得病。现有一种试剂可以检验患者是否得病，它的准确率是0.99，即在患者确实得病的情况下，它有99%的可能呈现阳性。它的误报率是5%，即在患者没有得病的情况下，它有5%的可能呈现阳性。现有一个病人的检验结果为阳性，请问他确实得病的可能性有多大？

解答：假定A事件表示得病，那么P(A)为0.001。这就是"先验概率"，即没有做试验之前，我们预计的发病率。再假定B事件表示阳性，那么要计算的就是P(A|B)。这就是"后验概率"，即做了试验以后，对发病率的估计。根据贝叶斯公式

$P(A|B)=P(A)*\frac{P(B|A)}{P(B)}$

用全概率公式改写分母

$P(A|B)=P(A)*\frac{P(B|A)}{P(B|A)P(A)+P(B|\bar{A})P(\bar{A})}$

我们得到了一个惊人的结果，P(A|B)约等于0.019。也就是说，即使检验呈现阳性，病人得病的概率，也只是从0.1%增加到了2%左右。这就是所谓的"假阳性"，即阳性结果完全不足以说明病人得病。

数学期望

在概率论和统计学中，数学期望(mean)（或均值，亦称期望）是试验中每次可能结果的概率乘以其结果的总和，是最基本的数学特征之一。它反映随机变量平均取值的大小。期望值是该变量输出值的平均数。

例子

某城市有10万个家庭，没有孩子的家庭有1000个，有一个孩子的家庭有9万个，有两个孩子的家庭有6000个，有3个孩子的家庭有3000个。

则此城市中任一个家庭中孩子的数目是一个随机变量，记为X。它可取值0，1，2，3。

其中，X取0的概率为0.01，取1的概率为0.9，取2的概率为0.06，取3的概率为0.03。

则，它的数学期望

，即此城市一个家庭平均有小孩1.11个，当然人不可能用1.11个来算，约等于2个。

概率密度函数

定义：设x为以随机变量，若存在非负实函数f(x),使对任意实数a<b，有

$p\{a\leq x\leq b\}=\int _a^bf(x)dx$

则称X为连续型随机变量，f(x)成为X的概率密度函数，简称概率密度或密度函数。

用于描述连续型随机变量所服从的概率分布。

概率和统计是一回事吗？

概率是已知模型和参数，推数据。统计是已知数据，推模型和参数。

概率分布，

在实际问题中，常常要研究一个随机变量ξ取值小于某一数值x的概率，这概率是x的函数，称这种函数为随机变量ξ的分布函数，简称分布函数，记作F(x)，即F(x)=P(ξ<x) (-∞<x<+∞)，由它并可以决定随机变量落入任何范围内的概率。例如在桥梁和水坝的设计中，每年河流的最高水位ξ小于x米的概率是x的函数，这个函数就是最高水位ξ的分布函数。

离散型随机变量的概率分布

对于离散型随机变量X，设为变量X的取值，而为对应上述取值的概率，则离散型随机变量X的概率分布为

且概率应满足条。因此，离散型随机变量X的概率分布函数为

连续型随机变量的概率分布

对于连续型随机变量，设变量X取值于区间(a，b)，并假设其分布函数F(x)为单调增函数，且在间可微分及其导数F’(x)在此区间连续，则变量X落在x至区间内的概率为

为描述其概率分布规律，这时不可能用分布列表示，而是引入“概率密度函数” 的新概念。定义概率分布函数F(x)的导数F’(x)为概率密度函数f(x)，即

于是连续型随机变量X的概率分布函数可写为常用的概率积分公式的形式：

这样，只要已知某一连续型随机变量X的概率分布密度函数f(x)，即可求得X落在某一区间内的概率：

正态分布

若随机变量X服从一个数学期望为μ、方差为σ^2的正态分布，记为N(μ，σ^2)

先验分布/后验分布/似然估计，

个人觉得下面的博客比较好理解

https://blog.csdn.net/qq_23947237/article/details/78265026

理解下来就是，先验分布就是由过去已经发生的事件推测当前时间发生的概率。，

后验分布就是由已经发生的结果推测当前事件发生的概率。

最大似然估计MLE和最大后验概率估计MAP

他们都是统计领域的问题。它们都是用来推测参数的方法，为什么会存在着两种不同方法呢？这需要理解贝叶斯思想。我们来看看贝叶斯公式。

$P(A|B) =\frac{P(B|A)P(A)}{P(B)} \ ........(1)$

贝叶斯公式看起来很简单，无非是倒了倒条件概率和联合概率的公式。

把B展开，可以写成：

$P(A|B) = \frac{P(B|A)P(A)}{P(B|A)P(A) + P(B|\bar{A})P( \bar{A})}.......(2))$

（ $\bar{A}$ 表示”非A”）,这个式子就很有意思了。

想想这个情况。一辆汽车（或者电瓶车）的警报响了，你通常是什么反应？有小偷？撞车了？不。。你通常什么反应都没有。因为汽车警报响一响实在是太正常了！每天都要发生好多次。本来，汽车警报设置的功能是，出现了异常情况，需要人关注。然而，由于虚警实在是太多，人们渐渐不相信警报的功能了

贝叶斯公式就是在描述，你有多大把握能相信一件证据？（how much you can trust the evidence）

我们假设响警报的目的就是想说汽车被砸了。把A计作“汽车被砸了”，B计作“警报响了”，带进贝叶斯公式里看。

我们想求等式左边发生A|B的概率，这是在说警报响了，汽车也确实被砸了。汽车被砸引起（trigger）警报响，即B|A。但是，也有可能是汽车被小孩子皮球踢了一下、被行人碰了一下等其他原因（统统计作∼A），其他原因引起汽车警报响了，即B|∼A。那么，现在突然听见警报响了，这时汽车已经被砸了的概率是多少呢（这即是说，警报响这个证据有了，多大把握能相信它确实是在报警说汽车被砸了）？想一想，应当这样来计算。用警报响起、汽车也被砸了这事件的数量，除以响警报事件的数量（这即【式1】）。进一步展开，即警报响起、汽车也被砸了的事件的数量，除以警报响起、汽车被砸了的事件数量加上警报响起、汽车没被砸的事件数量（这即【式2】）。

可能有点绕，请稍稍想一想。

再思考【式2】。想让P(A|B)=1，即警报响了，汽车一定被砸了，该怎么做呢？让P(B|∼A)P(∼A)=0即可。很容易想清楚，假若让P(∼A)=0，即杜绝了汽车被球踢、被行人碰到等等其他所有情况，那自然，警报响了，只剩下一种可能——汽车被砸了。这即是提高了响警报这个证据的说服力。

从这个角度总结贝叶斯公式：做判断的时候，要考虑所有的因素。老板骂你，不一定是你把什么工作搞砸了，可能只是他今天出门前和太太吵了一架。

从这个角度思考贝叶斯公式：一个本来就难以发生的事情，就算出现某个证据和他强烈相关，也要谨慎。证据很可能来自别的虽然不是很相关，但发生概率较高的事情。发现刚才写的代码编译报错，可是我今天状态特别好，这语言我也很熟悉，犯错的概率很低。因此觉得是编译器出错了。 ————别，还是先再检查下自己的代码吧。
似然函数

似然（likelihood）这个词其实和概率（probability）是差不多的意思，Colins字典这么解释：The likelihood of something happening is how likely it is to happen. 你把likelihood换成probability，这解释也读得通。但是在统计里面，似然函数和概率函数却是两个不同的概念（其实也很相近就是了）。
对于这个函数：

$P(x|\theta) ........(3)$

输入有两个：x表示某一个具体的数据；θ表示模型的参数。

如果θ是已知确定的，x是变量，这个函数叫做概率函数(probability function)，它描述对于不同的样本点x，其出现概率是多少。

如果x是已知确定的，θ是变量，这个函数叫做似然函数(likelihood function), 它描述对于不同的模型参数，出现x这个样本点的概率是多少。

这有点像“一菜两吃”的意思。其实这样的形式我们以前也不是没遇到过。例如 $f(x, y) = x^y$ 即x的y次方。如果x是已知确定的(例如x=2)，这就是 $f(y) = 2^y$ , 这是指数函数。如果y是已知确定的(例如y=2)，这就是 $f(x) = x^2$ ，这是二次函数。同一个数学形式，从不同的变量角度观察，可以有不同的名字。

最大似然估计（MLE）

考虑上面式子（3），作为似然函数，θ是变量，最大似然估计就是求取能够使得函数（3）取得最大值的变量θ。

最大后验概率估计

最大似然估计是求参数θ, 使似然函数 $P(x_0|\theta )$ 最大。最大后验概率估计则是想求θ使 $P(x_0 | \theta) P(\theta)$ 最大,求得的θ不单单让似然函数大，θ自己出现的先验概率也得大.MAP其实是在最大化 $P(\theta|x_0) = \frac{P(x_0|\theta)P(\theta)}{P(x_0)}$ ,不过因为x0是确定的, $P(x_0)$ 是一个已知值,

所以去掉了分母 $P(x_0)$ ,最大化P(θ|x0)的意义也很明确，x0已经出现了，要求θ取什么值使P(θ|x0)最大。顺带一提，P(θ|x0)即后验概率，这就是“最大后验概率估计”名字的由来。

置信度(belief)：

关于置信度和置信区间，感觉写的挺好的一篇文章为

https://baijiahao.baidu.com/s?id=1596169784713150436&wfr=spider&for=pc

置信度反映了机器人有关环境状态的内部信息，但状态不能直接测量，例如，在一个局部坐标系中机器人的位姿可能是 $x_t=<14.12, 12.7, 45^0>$ ,但是因为位姿（即使用GPS)不能直接测量，通常机器人不知道自己的位姿。而机器人是必须要从数据中推测出其位姿的。因此要从位姿的内部置信度（belief)识别出真正的状态。概率机器人通过条件概率分布表示置信度。对于真实的状态，置信度分布为每一个可能的假设分配一个概率（或者概率密度值）。置信度分布是以可获得数据为条件的关于状态变量的后验概率。这里用 $bel(x_t)$ 表示状态变量 $x_t$ 的置信度，其为下式后验概率的缩写：