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

第五部分：扩展卡尔曼滤波EKF

1. 扩展卡尔曼滤波推导：对$f(X_{k-1})$和$h(X_k)$进行线性化。

   • 设$X_{k-1}\sim N({\hat{x}}_{k-1}^{+},P_{k-1}^{+})$，这也是卡尔曼滤波的前提。

   • 预测步：$X_k=f(X_{k-1})+Q_k$，对$f(X_{k-1})$在${\hat{x}}_{k-1}^{+}$进行一阶泰勒展开，

     $f(X_{k-1})=f({\hat{x}}_{k-1}^{+})+f^{\prime }({\hat{x}}_{k-1}^{+})(X_{k-1}-{\hat{x}}_{k-1}^{+})\approx [f^{\prime }({\hat{x}}_{k-1}^{+})]X_{k-1}+[f({\hat{x}}_{k-1}^{+})-{\hat{x}}_{k-1}^{+}{f}^{\prime }({\hat{x}}_{k-1}^{+})]=A{X}_{k-1}+B$

     则预测方程改写为

     $X_k=A{X}_{k-1}+B+Q_k,A=f^{\prime }({\hat{x}}_{k-1}^{+}),B=f({\hat{x}}_{k-1}^{+})-f^{\prime }({\hat{x}}_{k-1}^{+}){\hat{x}}_{k-1}^{+}$，即$f(v)=Av+B$

     因此

     $f_k^{-}(x)=N(A{\hat{x}}_{k-1}^{+}+B,A^2{P}_{k-1}^{+}+Q)=N(f({\hat{x}}_{k-1}^{+}),A^2{P}_{k-1}^{+}+Q)$，即$X_k^{-}\sim N(f({\hat{x}}_{k-1}^{+}),A^2{P}_{k-1}^{+}+Q)$

     ${\hat{x}}_k^{-}=f({\hat{x}}_{k-1}^{+}),P_k^{-}=A^2{P}_{k-1}^{+}+Q$，不用算B

   • 更新步：$Y_k=h(X_k)+R_k$，对$h(X_k)$在${\hat{x}}_k^{-}$点进行一阶泰勒展开，

     $h(X_k)\approx h({\hat{x}}_k^{-})+h^{\prime }({\hat{x}}_k^{-})(X_k-{\hat{x}}_k^{-})=C{X}_k+D$，其中$C=h^{\prime }({\hat{x}}_k^{-}),D=h({\hat{x}}_k^{-})-h^{\prime }({\hat{x}}_k^{-}){\hat{x}}_k^{-}$

     因此

     $f_k^{+}=N(\frac{R{\hat{x}}_k^{-}+C{P}_k^{-}(y_k-D)}{R+C^2{P}_k^{-}},(1-\frac{C^2{P}_k^{-}}{R+C^2{P}_k^{-}})P_k^{-})$

     定义卡尔曼增益$K=\frac{C{P}_k^{-}}{R+C^2{P}_k^{-}}$，则$X_k^{+}\sim N({\hat{x}}_k^{-}+K[y_k-h({\hat{x}}_k^{-})],(1-KC)P_k^{-})$

     ${\hat{x}}_k^{+}={\hat{x}}_k^{-}+K[y_k-h({\hat{x}}_k^{-}))]$，$P_k^{+}=(1-KC)P_k^{-}$，不用算D

2. 扩展卡尔曼滤波算法流程

   • 设$X_{k-1}\sim N({\hat{x}}_{k-1}^{+},P_{k-1}^{+})$
   • 预测步
     • $A=f^{\prime }({\hat{x}}_{k-1}^{+})$
     • ${\hat{x}}_k^{-}=f({\hat{x}}_{k-1}^{+})$
     • $P_k^{-}=A^2{P}_{k-1}^{+}+Q$
   • 更新步
     • $C=h^{\prime }({\hat{x}}_k^{-})$
     • $K=\frac{C{P}_k^{-}}{C^2{P}_k^{-}+R}$
     • ${\hat{x}}_k^{+}={\hat{x}}_k^{-}+K[y_k-h({\hat{x}}_k^{-})]$
     • $P_k^{+}=(1-KC)P_k^{-}$

   矩阵形式：$\{\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-1}\sim N({\hat{x}}_{k-1}^{+},{\Sigma }_{k-1}^{+})$

   • 预测步：

     • $A$是f对向量${\hat{x}}_{k-1}^{+}$的雅可比矩阵

     

     • ${\hat{x}}_{k-1}^{-}=f({\hat{x}}_{k-1}^{+})$
     • ${\Sigma }_k^{-}=A{\Sigma }_{k-1}^{+}{A}^T+Q$，$Q$为$Q_k$的协方差矩阵

   • 观测步：

     • C是h对向量${\hat{x}}_{k-1}^{-}$的雅可比矩阵，注意可能不是方阵，因为观测维数可能少于状态。

   

   • $K={\Sigma }_k^{-}{C}^T(C{\Sigma }_k^{-}{C}^T+R{)}^{-1}$

   • 观测到一组数据 $Y$，${\hat{x}}_k^{+}={\hat{x}}_k^{-}+K[Y-h({\hat{x}}_k^{-})]$

   • ${\Sigma }_k^{+}=(I-KC){\Sigma }_k^{-}$

第六部分：无迹卡尔曼滤波

1. 无迹卡尔曼滤波：处理非线性问题

   思路：设$X\sim N(0,1)$，$Y=f(X)$。

   • EKF：对函数线性化近似，$f(X)=f(0)+f^{\prime }(0)(X-0)$
   • UKF：对PDF近似$Y=f(X)\Rightarrow E(Y)=E(f(x)),E(Y^2)=E([f(x){]}^2)$。若能通过某种方式直接求出Y的期望和方差，然后用一个期望和方差与之相同的正态分布去近似它，则能将非线性问题转化为普通卡尔曼滤波问题。

   期望与方差的求法：设$X\sim N(0,1),Y=g(X),$ 求$E(Y),D(Y)$

   • 法1 精确解：不好算
     $$\begin{gathered} E(Y)=E[g(X)]={\int }_{-\infty }^{\infty }g(x)\frac{1}{\sqrt{2\pi }}exp(-x^2/2)dx \\ E(Y^2)={\int }_{-\infty }^{\infty }[g(x){]}^2\frac{1}{\sqrt{2\pi }}exp(-x^2/2)dx \\ D(Y)=E(Y^2)-[E(Y){]}^2 \end{gathered}$$

   • 法2 无迹变换 unscented transform, UT：正态分布特征 $\mu ,{\sigma }^2$，对称性，因此取3个带权重粒子近似代替$N(0,1)$，求取Y的期望和方差进而近似出Y的正态分布。

     $x_1=E(X),x_2=E(X)-\sqrt{(1+\lambda )D(X)},x_3=E(X)+\sqrt{(1+\lambda )D(X)}$

     $w_1=\frac{\lambda }{1+\lambda },w_2=\frac{1}{2(1+\lambda )},w_3=\frac{1}{2(1+\lambda )}$，$\lambda$为大于零的参数任取。

     例：设$X\sim N(0,1)$，取$x_1=0,x_2=\sqrt{1+\lambda },x_3=-\sqrt{1+\lambda }$，$\lambda $越大，粒子散得越开。

     则$\sum _{i=1}^3{w}_i{x}_i=0,\sum _{i=1}^3{w}_i(x_i-0{)}^2=1$

     $Y=f(X)\Rightarrow y_i=f(x_i),y_1=f(x_1)=f(0),y_2=f(x_2)=f(\sqrt{1+\lambda }),y_3=f(x_3)=f(-\sqrt{1+\lambda })$

     $E(Y)=\sum _i{w}_i{y}_i$，$D(Y)=\sum _i{w}_i(y_i-E(Y){)}^2$

     精度问题：EKF具有一阶精度，UKF具有二阶精度

     • 设$X\sim N(0,1),Y=X^2$，则$Y\sim \chi (1)$自由度为1的卡方分布$E(Y)=1,D(Y)=2$
     • EKF：若$Y=f(X)$是二阶以上函数，EKF不能还原$E(Y),D(Y)$
     • UKF：若$f(X)=a{X}^2+bX+C$，则UT可以还原$E(Y)$，但不能还原$D(Y)$，即$X\to x_1,x_2,x_3\to y_1,y_2,y_3\to E(Y)=\sum w_i{y}_i$，若$f(x)$高于二阶，则$E(Y),D(Y)$均不能还原。

   无迹卡尔曼滤波思想：对核心方程$\{\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-1}$是正态分布，$f(X_{k-1})$不一定是正态分布$\stackrel{无迹变换UT}{\to }f(X_{k-1})$近似为正态。又因为$Q_k$是正态分布，则$X_k$也是正态分布。
   • 观测方程：$X_k$是正态分布，$h(X_k)$不一定是正态分布$\stackrel{无迹变换UT}{\to }h(X_k)$也近似为正态分布。
   • 所有随机变量都是正态分布，即可以用kalman Filter处理。

2. 无迹卡尔曼滤波算法

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

   • 将$f(X_{k-1})$正态化：
     $$\begin{gathered} x_1={\mu }_{k-1}^{+},x_2={\mu }_{k-1}^{+}+\sqrt{(1+\lambda ){\sigma }_{k-1}^{2+}},x_3={\mu }_{k-1}^{+}-\sqrt{(1+\lambda ){\sigma }_{k-1}^{2+}} \\ {w}_1=\frac{1}{1+\lambda },w_2=w_3=\frac{1}{2(1+\lambda )} \\ {x}_k^{-(i)}=f(x_{k-1}^{+(i)}),{\hat{x}}_k^{-(i)}=\sum _{i=1}^3{w}_{i}f(x_k^{+(i)}),P_k^{-}=\sum _{i=1}^3{w}_i(x_k^{-(i)}-{\hat{x}}_k^{-(i)}{)}^2 \\ 认为f(X_{k-1})\sim N({\hat{x}}_k^{-},P_k^{-}) \end{gathered}$$

   • 预测步：$X_k=f(X_{k-1})+Q_k$，$f(X_{k-1})\sim N({\hat{x}}_k^{-},P_k^{-})$，$Q_k\sim N(0,Q)$，$\Rightarrow X_k\sim N({\hat{x}}_k^{-},P_k^{-}+Q)$

   • 将$h(X_k)$​正态化：$\sim N({\hat{x}}_k^{-},P_k^{-}+Q)$

     $$\begin{gathered} x_k^{(1)}={\hat{x}}_k^{-},x_2={\hat{x}}_k^{-}+\sqrt{(1+\lambda )(P_k^{-}+Q)},x_3={\hat{x}}_k^{-}-\sqrt{(1+\lambda )(P_k^{-}+Q)} \\ {w}_1=\frac{1}{1+\lambda },w_2=w_3=\frac{1}{2(1+\lambda )} \\ {y}_k^{(i)}=h(x_k^{(i)}),{\hat{y}}_k=\sum _{i=1}^3{w}_i{y}_k^{(i)},P_y^{-}=\sum _{i=1}^3{w}_i(y_k^{(i)}-{\hat{y}}_k{)}^2 \\ 认为h(X_k)\sim N({\hat{y}}_k,P_y) \end{gathered}$$

   • 更新步：
     $$\begin{gathered} P_{xy}=\sum _i{w}_i(x_k^{(i)}-{\hat{x}}_k^{-})(y_k^{(i)}-\hat{y}),K=P_{xy}(P_y+R{)}^{-1} \\ 观测到一个数据y_m后 \\ {\hat{x}}_k^{+}={\hat{x}}_k^{-}+K(y_m-\hat{y}) \\ {P}_k^{+}=P_k^{-}+Q-K^2(P_y+R) \end{gathered}$$

3. 矩阵形式无迹卡尔曼滤波算法

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

   • $f(X_{k-1})$正态化

     • $x_{k-1}^{(1)}={\mu }_{k-1}$

     • 矩阵的$L{L}^T$分解，${\Sigma }_{k-1}^{+}=L{L}^T$，因此协方差矩阵${\Sigma }_{k-1}^{+}$必须正定，否则会崩溃。

     • for i=1:n

       $x_{k-1}^{(i+1)}={\mu }_{k-1}^{+}+\sqrt{(n+\lambda )}{L}_{(i)}$，$L_{(i)}$ 表示L的第i列

       $x_{k-1}^{(i+1+n)}={\mu }_{k-1}^{+}-\sqrt{(n+\lambda )}{L}_{(i)}$

       end

       $w_k^{(1)}=\frac{\lambda }{n+\lambda },w_k^{(2......2n+1)}=\frac{1}{2(n+\lambda )}$

     • $x_k^{-(i)}=f(x_{k-1}^{(i)})$，不需要加随机数

     • ${\hat{x}}_k^{-}=\sum _{i=1}^{2n+1}{w}_i{x}_k^{-(i)}$

     • $P_k^{-}=\sum _{i=1}^{2n+1}{w}_i(x_k^{-(i)}-{\hat{x}}_k^{-})(x_k^{-(i)}-{\hat{x}}_k^{-}{)}^T+Q$

   • 预测步

     • 得到$X_k^{-}$的近似pdf，$\sim N({\hat{x}}_k^{-},P_k^{-})$

   • $h(X_k)$正态化

     • $x_k^{(1)}={\hat{x}}_k^{-}$

     • $P_k^{-}=L_1{L}_1^T$

     • for i = 1:n

       $x_k^{(i+1)}=x_k^{-}+\sqrt{n+\lambda }{L}_{1(i)}$

       $x_k^{(i+1+n)}=x_k^{-}-\sqrt{n+\lambda }{L}_{1(i)}$

       end

     • $y_k^{(i)}=h(x_k^{(i)})$

     • ${\hat{y}}_k=\sum _{i=1}^{2n=1}{w}_i{y}_k^{(i)}$

     • $P_y=\sum _{i=1}^{2n+1}{w}_i(y_k^{(i)}-{\hat{y}}_k)(y_k^{(i)}-{\hat{y}}_k{)}^T+R$

     • $P_{xy}=\sum _{i=1}^{2n+1}{w}_i(x_k^{(i)}-{\hat{x}}_k^{-})(x_k^{(i)}-{\hat{x}}_k^{-}{)}^T$

     • $K=P_{xy}(P_y{)}^{-1}$

   • 更新步

     • 观测到一组数据$y_m$

     • ${\hat{x}}_k^{-}={\hat{x}}_k^{+}+K(y_m-{\hat{y}}_k)$

     • $P_k^{+}=P_k^{-}-K{P}_y{K}^T$