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

基于B站老王的贝叶斯滤波、卡尔曼滤波、粒子滤波 Bilibili

第三部分：随机过程的贝叶斯滤波BF

1. 随机过程包含一系列随机变量$X_0,X_1,Y_1,X_2,Y_2...X_k,Y_k$，有一个初值$x_0$，有k个观测值$y_1,y_2,...,y_k$

   怎么办：

   • 所有的$X_0,...,X_k$的先验概率都靠猜：过于依赖观测，放弃了预测信息。放弃了前后状态间的关系如$x_k=x_{k-1}+Q_k$ 。
   • 只有$X_0$的概率是猜的，$X_1,...,X_k$的先验概率靠递推（预测）$\surd$

   怎么做：

   • 状态方程+观测方程（建模）

   状态方程：描述$X_k$和$X_{k-1}$是什么关系$X_k=f(X_{k-1})+Q_k$，$Q_k$：预测噪声
   $$如X_k=\frac{1}{2}g{t}^2+Q\stackrel{一阶Tylor展开}{\to }{X}_k=X_{k-1}+{\dot{X}}_{k-1}(t_k-t_{k-1})+Q=X_{k-1}+gt(t_k-t_{k-1})+Q$$
   状态方程可以建立的非常粗糙，只需要将方差Q调大，如$X_k=X_{k-1}+Q,Q\sim N(0,1000)$。

   观测方程：反映了状态（温度、速度、角度）是如何引起传感器的读数，$Y_k=h(X_k)+R_k$，$R_k$：观测噪声

   • 如状态：温度，观测：温度，$Y_k=X_k+R_k$，温度+传感器误差。

   • 状态：位移，观测：角度，$Y_k=arcsin{X}_k+R_k$
     $$\{\begin{array}{l}\begin{array}{rl}{x}_k& =f(x_{k-1})+Q_k随机过程预测\\ z_{k,j}& =h(x_k)+R_k观测\end{array}\end{array}$$

   怎么递推：

   • 若无观测，设$X_K=2X_{k-1}$，首先设$X_0\sim N(0,1)$，那么$X_1=2X_0\sim N(0,2^2),X_2=2X_1\sim N(0,2^4)$，方差越来越大，上一步的先验概率作为下一步的先验概率。
   • 正确地递推：设$先验X_0\sim N(0,1)\stackrel{预测步}{\to }先验X_1^{-}\sim N(0,2^2)\stackrel{用观测y_1=0和贝叶斯公式更新步/后验步}{\to }后验/先验X_1^{+}\sim N(0,0.8)\stackrel{再预测}{\to }先验X_2^{-}\sim N(0,3.2)\stackrel{观测y_2=0.1再更新}{\to }后验X_2^{+}...$，上一步的后验概率作为下一步的先验概率。
     • 预测步：上一时刻的后验$\stackrel{状态方程}{\to }$这一时刻的先验
     • 更新步：这一时刻的先验$\stackrel{观测方程}{\to }$这一时刻的后验也是下一时刻的先验

2. 贝叶斯滤波算法推导

   基础：

   • 原料：$\{\begin{array}{l}\begin{array}{rl}{X}_k& =f(X_{k-1})+Q_k随机过程\\ Y_k& =h(X_k)+R_k观测\end{array}\end{array}$，其中$X_k,X_{k-1},Y_k,Q_k,R_k$都是随机变量。
   • $\star $\ast \ast 重要假设\ast \ast ：$X_0,Q_1,…,Q_k,R_1,…,R_k$相互独立
   • 一系列观测值：$y_1,...y_k$
   • 设初值$X_0\sim f_0(x)，Q_k\sim f_{Q_k}(x)，R_k\sim f_{R_k}(x)$
   • 重要定理：条件概率里的条件可以做逻辑推导$P(X=1|Y=2,Z=3)=P(X+Y=3|Y=2,Z=3)=P(X+Y=3|Y=2,Z-Y=1)$

   预测步：$f_1^{-}(x)={\int }_{-\infty }^{\infty }{f}_{Q_1}[x-f(v)]f_0(v)dv$

   更新步：$f_1^{+}(x)=\eta f_{R_1}[y_1-h(x)]f_1^{-}(x)$

   总结：

   $f_0(x)\to f_1^{-}(x)={\int }_{-\infty }^{\infty }{f}_{Q_1}[x-f(v)]f_0(v)dv\to f_1^{+}(x)=\eta f_{R_1}[y_1-h(x)]f_1^{-}(x)\to f_2^{-}(x)={\int }_{-\infty }^{\infty }{f}_{Q_2}[x-f(v)]f_1^{+}(v)dv\to f_2^{+}(x)=\eta f_{R_2}[y_2-h(x)]f_1^{-}(x)\to f_2^{-}(x)\to ...$，其中$\eta$不相等，但是不影响。

   $f\to X_k=f(X_{k-1})+Q_k$，$f_0(x)$是初始猜的pdf

   $f_1^{-},f_2^{-}$是预测步的pdf，先验概率

   $f_1^{+},f_2^{+}$是更新步的pdf，后验概率也是下一步的先验概率

   $f_{Q_k},f_{R_k}$是预测与观测误差的pdf

   可证：$Q_k$与$X_{k-1}$独立，$X_k$与$R_k$独立

3. 贝叶斯滤波完整算法：

   • 设初值$X_0$的pdf$f_0(x)$
   • 预测步：$f_k^{-}(x)={\int }_{-\infty }^{\infty }{f}_{Q_k}[x-f(v)]f_{k-1}^{+}(v)dv$
   • 更新步：$f_k^{+}(x)={\eta }_k{f}_{R_k}[y_k-h(x)]f_k^{-}(x)$，${\eta }_k=({\int }_{-\infty }^{\infty }{f}_{R_k}[y_k-h(x)]f_k^{-}(x)dx{)}^{-1}$
   • 对后验概率$f_k^{+}(x)$分布估计期望：${\hat{X}}_k^{+}={\int }_{-\infty }^{\infty }x{f}_k^{+}(x)dx$

   缺点：算$\eta$，算期望涉及无穷积分，大多数情况下无解析解。

   解决：

   • 做假设

     • 假设$f(x_{k-1}),h(x_k)$为线性，$Q_k,R_k$是正态分布 $\Rightarrow$ Kalman Filter, KF
     • 假设$f(x_{k-1}),h(x_k)$非线性，$Q_k,R_k$是正态分布 $\Rightarrow$ EKF，UKF

   • 直接对无穷积分做数值积分

     • 高斯积分
     • 蒙特卡洛Monte Karlo 积分，PF
     • 直方图滤波

第四部分：卡尔曼滤波KF

1. 贝叶斯滤波基础上做假设：

   • 假设$f(x_{k-1}),h(x_k)$为线性，$f(X_{k-1})=F\cdot X_{k-1},h(X_k)=h\cdot X_k$
   • $Q_k,R_k$是正态分布，$Q\sim N(0,Q),R\sim N(0,R)$

2. 卡尔曼滤波流程：

   • 设$X_{k-1}\sim N({\mu }_{k-1}^{+},{\sigma }_{k-1}^{+})$

   • 预测步：
     $$\begin{gathered} f_k^{-}(x)={\int }_{-\infty }^{\infty }{f}_{Q_k}[x-f(v)]f_{k-1}^{+}(v)dv \\ ={\int }_{-\infty }^{\infty }(2\pi Q{)}^{-1/2}{e}^{-\frac{(x-F\cdot v)}{2Q}}\cdot (2\pi {\sigma }_{k-1}^{+}{)}^{-1/2}{e}^{-\frac{(v-{\mu }_{k-1}^{+}{)}^2}{2{\sigma }_{k-1}^{+}}}dv \\ \Rightarrow X_k^{-}\sim N(F{\mu }_{k-1}^{+},F^2{\sigma }_{k-1}+Q)\stackrel{记为}{=}N({\mu }_k^{-},{\sigma }_k^{-}) \end{gathered}$$
     解法：卷积、傅里叶变换$N(\mu ,{\sigma }^2)\stackrel{F.T}{\to }{e}^{i\mu t-{\sigma }^2{t}^2/2}$

     对比可得到与上一步之间的关系：

     • ${\mu }_k^{-}=F{\mu }_{k-1}^{+}$ （1）
     • ${\sigma }_k^{-}=F^2{\sigma }_{k-1}^{+}+Q$ （2）

   • 更新步：
     $$\begin{gathered} f_k^{+}(x)={\eta }_k{f}_{R_k}[y_k-h(x)]f_k^{-}(x) \\ =\eta (2\pi R{)}^{-1/2}{e}^{-\frac{(y_k-hx{)}^2}{2R}}\cdot (2\pi {\sigma }_k^{-}{)}^{-1/2}{e}^{-\frac{(x-{\mu }_k^{-}{)}^2}{2{\sigma }_k^{-}}} \\ \Rightarrow X_k^{+}\sim N(\frac{h{\sigma }_k^{-}{y}_k+R{\mu }_k^{-}}{h^2{\sigma }_k^{-}+R},\frac{R{\sigma }_k^{-}}{h^2{\sigma }_k^{-}+R})\stackrel{记为}{=}N({\mu }_k^{+},{\sigma }_k^{+}) \\ \eta =({\int }_{-\infty }^{\infty }(2\pi R{)}^{-1/2}{e}^{-\frac{(y_k-hx{)}^2}{2R}}\cdot (2\pi {\sigma }_k^{-}{)}^{-1/2}{e}^{-\frac{(x-{\mu }_k^{-}{)}^2}{2{\sigma }_k^{-}}}{)}^{-1} \end{gathered}$$
     对比可得到与上一步之间的关系：

• ${\mu }_k^{+}=\frac{h{\sigma }_k^{-}}{h^2{\sigma }_k^{-}+R}(y_k-h{\mu }_k^{-})+{\mu }_k^{-}={\mu }_k^{-}+K(y_k-h{\mu }_k^{-})$ （3）

• ${\sigma }_k^{-}=(1-\frac{h^2{\sigma }_k^{-}}{h^2{\sigma }_k^{-}+R}){\sigma }_k^{-}=(1-Kh){\sigma }_k^{-}$ （4）

• 简化（3）（4），设卡尔曼增益$K=\frac{h{\sigma }_k^{-}}{h^2{\sigma }_k^{-}+R}$ （5）

  当$R\gg {\sigma }_k^{-}$时，$K\to 0,{\mu }_k^{+}={\mu }_k^{-}$，即相信预测，观测误差太大观测远远不可信。

  当$R\ll {\sigma }_k^{-}$，$K\to \frac{1}{h},{\mu }_k^{+}=\frac{y_k}{h}\Rightarrow y_k=h{x}_k+R$，即相信观测，观测误差极小。

3. 矩阵形式的卡尔曼滤波：${\mu }_k\to {\vec{\mu }}_k$，均值向量；${\sigma }_k\to {\Sigma }_k$协方差矩阵，F为矩阵，H为矩阵，Q，R为协方差矩阵，$F^2\sigma \to F\sigma F^T$

   • ${\vec{\mu }}_k^{-}=F{\vec{\mu }}_{k-1}^{+}$
   • ${\vec{\Sigma }}_k^{-}=F{\Sigma }_{k-1}^{+}{F}^T+Q$
   • $K={\Sigma }_k^{-}{H}^T(H{\Sigma }_k^{-}{H}^T+R{)}^{-1}$
   • ${\vec{\mu }}_k^{+}={\vec{\mu }}_k^{-}+K({\vec{y}}_k-H{\mu }_k^{-})$
   • ${\Sigma }_k^{+}=(I-KH){\Sigma }_k^{-}$

4. 马尔科夫性和观测独立性

   证明：若$\{\begin{array}{l}\begin{array}{rl}{X}_k& =f(X_{k-1})+Q_k预测\\ Y_k& =h(X_k)+R_k观测\end{array}\end{array}$，$X_0,Q_1,...,Q_k,R_1,...,R_k$相互独立，则

   • 马尔科夫性：$P(X_k<x_k|X_{k-1}=x_{k-1},X_{k-2}=x_{k-2},...,X_0=x_0)=P(X_k=x_k|X_{k-1}=x_{k-1})$，只与上一时刻状态有关。
   • 观测独立性：$P(Y_k=y_k|X_k=x_k,X_{k-1}=x_{k-1},...,X_0=x_0)=P(Y_k=y_k|X_k=x_k)$，观测只与此刻状态有关。

5. 卡尔曼滤波的应用——编程：

   • 有一个带噪声的信号，用KF滤波。
   • 传感器融合：已知$X=t^2$为信号，有两个不同的传感器对X进行观测，产生了$y_{a1},...,y_{ak},y_{b1},...,y_{bk}$，观测方程$Y_{ak}=X_k+R_{ak},Y_{bk}=X_k+R_{bk}$，其中$R_{ak}\sim N(0,1),R_{bk}\sim N(0,2)$，求传感器融合后的${\hat{X}}^{+}={\int }_{-\infty }^{\infty }x{f}^{+}(x)dx$