【SLAM基础入门】贝叶斯滤波、卡尔曼滤波、粒子滤波笔记（1）

贝叶斯滤波、卡尔曼滤波、粒子滤波 (https://www.bilibili.com/video/BV1HT4y1577g?spm_id_from=333.999.header_right.history_list.click)
SLAM基础入门，滤波相关知识，基于b站老王视频的学习笔记

第一部分：随机过程

1. 确定过程：如自由落体$v=gt$，$v_1,v_2$各个时刻的v都是确定的。

   随机过程：$x_1,x_2,x_3...$都是随机变量，但是不独立的，都是有联系的。（无法做随机试验）

   随机试验：在相同条件下，试验可重复进行（随机试验之间相互独立）；一次试验结果不确定，所有可能的结果可知；试验之前，试验结果预先未知。给事件的概率赋值，如抛硬币。频率$\to$大数定律，均值收敛到期望。

   大数定律：在n次独立试验中，对于任意正数$ϵ$有$\underset{h\to \infty }{\lim }P(|\frac{\mu }{n}-P_1|<ϵ)=1$ , 当$n\to \infty$时， $\frac{\mu }{n}$依概率收敛于$P_1$

   不独立的随机试验：如股市，想对股票做随机试验，必须做时光倒流。分子扩散、气温变化。时光一去不复返。

   不独立随机变量之间的关系：$x_k=f(x_{k-1}),P(x_k)=f(P(x_{k-1}))$ 初值$P(x_1)$的选取很重要。有的初值可以做随机试验，如随机游走 $x_k=x_{k-1}+D$，规定起点$P(x_0=0)=1$ 。

   利用主观概率获得初值：设有随机过程$x_1,x_2,..x_n$，有关系$x_K=f(x_{k-1})$，不同的主观概率将导致不同的结果，因此引入外部观测（信息，证据）来对过程进行更新。如股票概率，有内部消息，减弱主观的影响。

   主观概率（先验概率，先于实验之前，经验）$\stackrel{外部观测}{\to }$ 相对客观的概率，即后验概率（后测概率）。

2. 举例：温度

   • 先验概率分布：$P(T=10)=0.8$,$P(T=11)=0.2$ —— 真实值可能是10，可能是11。

   • 温度计读数 $T_{measure}=10.3^{o}C$

   • 后验概率分布：用贝叶斯公式，$P(T=10|T_{measure}=10.3)=\frac{P(T_{measure}=10.3|T=10)P(T=10)}{P(T_{measure}=10.3)}$表示测量值为10.3的情况下，真实值为10的概率

   • 似然概率$P(T_{measure}=10.3|T=10)$代表观测的准确度，当真实温度为10时，温度计测得温度为10.3的概率。定义为似然，即传感器精度（客观存在），表示哪个原因最有可能导致了结果。

     例子：A班99男1女，B班1男99女，随机抽一个班，再抽一个人进行观测，结果是女。此女最有可能是从B班抽出来的。

   • $P(T_{measure}=10.3)$表示测量值是10.3的概率。全概率公式展开 $P(T_m=10.3)=P(T_m=10.3|T=10)+P(T_m=10.3|T=11)P(T=11)$

     **$\star$ $P(T_m=10.3)$与T的取值无关，与T的分布律（先验概率）有关，而先验概率已经给定，因此其与真实值T无关，所以将其倒数视作常数$\eta$。**简化贝叶斯公式$P(T=10|T_{measure}=10.3)=\frac{P(T_{measure}=10.3|T=10)P(T=10)}{P(T_{measure}=10.3)}=\eta P(T_{measure}=10.3|T=10)P(T=10)$ 即

     $后验=\eta 似然\times 先验⟺P(状态|观测)=\eta P(观测|状态)P(状态)$，状态是因，观测是果。

第二部分：连续随机变量的贝叶斯公式

1. 离散随机变量贝叶斯公式：$P(X=x|Y=y)=\frac{P(Y=y|X=x)P(X=x)}{P(Y=y)}$

   连续随机变量贝叶斯公式：
   $$\begin{gathered} P(X<x|Y=y)={\int }_{-\infty }^x{f}_{X|Y}(x|y)dx={\int }_{-\infty }^x\frac{f_{Y|X}(y|x)f_X(x)}{f_Y(y)}dx \\ \Rightarrow f_{X|Y}(x|y)=\frac{f_{Y|X}(y|x)f_X(x)}{f_Y(y)} \end{gathered}$$
   其中$f_Y(y)={\int }_{-\infty }^{\infty }f(y,x)dx$ 即联合概率密度与边缘概率密度的关系，求边缘。
   $$\begin{gathered} 用条件概率\Rightarrow f_Y(y)={\int }_{-\infty }^{\infty }f(y,x)dx={\int }_{-\infty }^{\infty }{f}_{Y|X}(y|x)f_X(x)dx=C \\ \eta ={\int }_{-\infty }^{\infty }\frac{1}{f_{Y|X}(y|x)f_X(x)dx}dx归一化得到\Rightarrow f_{X|Y}(x|y)=\frac{f_{Y|X}(y|x)f_X(x)}{f_Y(y)}=\eta f_{Y|X}(y|x)f_X(x) \end{gathered}$$

2. 例子：测温

   • 猜一下多少度，即给定先验概率密度：$f_X(x)=\frac{1}{\sqrt{2\pi }}{e}^{-\frac{(x-10{)}^2}{2}}$，倾向于认为$X=10$是最有可能的温度，自信程度为方差。

   • 温度计测一下，观测：$y=9$

   • 想求后验概率密度$f_{X|Y}(x|9)$，想用贝叶斯公式那么要知道似然概率$f_{Y|X}(y|x)$。

     $$f_{Y|X}(y|x)\cdot ϵ=P(y<Y<y+ϵ|X=x)\Rightarrow f_{Y|X}(y|x)=\underset{ϵ\to 0}{\lim }\frac{P(y<Y<y+ϵ|X=x)}{ϵ}$$
     即传感器的精度。温度计精度$\pm 0.2$，真实值为x，则$y=x\pm 0.2$。用概率描述，即$P(x-0.2<Y<x+0.2|X=x)={\int }_{y=x-0.2}^{y=x+0.2}{f}_{Y|X}(y|x)dy$较大，其他地方概率较小。

   • 设似然概率为均值为y（观测值），方差为0.2（置信程度）的正态分布，即$f_{Y|X}(y=9|x)=\frac{1}{\sqrt{2\pi }\cdot 0.2}{e}^{-\frac{(9-x{)}^2}{2\cdot 0.2^2}}$

   • 因此可计算得到后验概率$f_{X|Y}(x|9)=\eta \frac{1}{2\pi \cdot 0.2}{e}^{-\frac{1}{2}[(x-10{)}^2+\frac{(9-x{)}^2}{0.2^2}]}\sim N(9.0385,0.038^2)$，其中$\eta =({\int }_{-\infty }^{\infty }\frac{1}{2\pi \cdot 0.2}{e}^{-\frac{1}{2}[(x-10{)}^2+\frac{(9-x{)}^2}{0.2^2}]}{)}^{-1}$。

   • 综上，先验概率$\sim N(10,1)$，似然概率$\sim N(9,0.2^2)$，后验概率$\sim N(9.0385,0.038^2)$，方差显著降低，不确定度减小，即滤波。

   • 似然模型：

     （1）等可能型 $f_{Y|X}(y|x)=C$
     

     （2）阶梯型 推广：直方图滤波（非线性卡尔曼滤波）

（3）正态分布型$\surd$：方差取传感器精度即0.2。

3. 定理：若先验概率$f_X(x)\sim N({\mu }_1,{\sigma }_1^2)$，似然概率$f_{Y|X}(y|x)\sim N({\mu }_2,{\sigma }_2^2)$，则后验概率$f_{X|Y}(x|y)\sim N(\frac{{\sigma }_1^2}{{\sigma }_1^2+{\sigma }_2^2}{\mu }_2+\frac{{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}{\mu }_1,\frac{{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2})$，理解为权重。方差大权重小。

   • 若${\sigma }_1^2\gg {\sigma }_2^2$，即传感器精度极高或瞎猜，后验$\sim N({\mu }_2,{\sigma }_2^2)$倾向于观测值。

   • 若${\sigma }_1^2\ll {\sigma }_2^2$，即猜的贼准或者传感器精度极低，后验$\sim N({\mu }_1,{\sigma }_1^2)$倾向于预测值（先验）。

     后验方差 $\frac{{\sigma }_1^2{\sigma }_2^2}{{\sigma }_1^2+{\sigma }_2^2}={\sigma }_1^2\frac{1}{1+{\sigma }_1^2/{\sigma }_2^2}={\sigma }_2^2\frac{1}{1+{\sigma }_2^2/{\sigma }_1^2}$，即滤波后方差比之前任何一个都小，观测准确将削弱先验的影响。传感器融合的原理。

4. 狄拉克函数$\delta (x)$：处理传感器无限精度，即方差$\delta \to 0$的问题。实质上是离散的必然事件的概率密度。

   似然概率$f_{Y|X}(y|x)=\frac{1}{\sqrt{2\pi }\cdot \sigma }{e}^{-\frac{(y-x{)}^2}{2\cdot {\sigma }^2}}$，当$\sigma \to 0$时，$f_{Y|X}(y|x)=\delta (x)$，收缩成尖峰。将一个函数收缩成一个点

   狄拉克函数的选择性：${\int }_{-\infty }^{+\infty }f(x)\sigma (x-a)=f(a)$

   

   例子：设先验$\sim N(\mu ,{\sigma }^2)$，观测$y=10$，似然$\sim \delta (10-x)$无限精度。则后验$f_{X|Y}(x|y)=\eta \delta (10-x)\frac{1}{\sqrt{2\pi }\sigma }{e}^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}}=\delta (10-x)\cdot e^{-\frac{(x-\mu {)}^2}{2{\sigma }^2}+\frac{(10-\mu {)}^2}{2{\sigma }^2}}$。测到10，实际为10的概率为1。