KF EKF ESKF
预备：
- 贝叶斯公式：\(P(x,y)=P(x|y)P(y)=P(y|x)P(x),P(x|y)=\frac {P(y|x)P(x)}{P(y)}\)
贝叶斯公式中融合多个观测：
\(P(x|y,z)=\frac {P(x,y,z)}{P(y,z)}=\frac {P(y|x,z)P(x,z)}{P(y,z)}=\frac {P(y|x,z)P(x|z)P(z)}{P(y|z)P(z)}=\frac {P(y|x,z)P(x|z)}{P(y|z)}\)
- 高斯分布
- 随机变量\(x\sim N(u,\sigma)\)，概率密度函数为：\(p(x)=\frac {1}{\sqrt {2pi} \sigma}exp(-\frac {1}{2}\frac {(x-u)^2}{\sigma^2})\)
高维形式为：\(p(x)=\frac {1}{\sqrt {(2pi)^N det(\Sigma)} }exp(-\frac {1}{2}(x-u)^T\Sigma^{-1}(x-u))\)
- 高斯分布的线性运算：\(x\sim N(u_x,\Sigma_{xx}),y=Gx,y\sim N(u_y,\Sigma_{yy})=N(Gu_x,G\Sigma_{xx}G^T)\)
推导：\(u_y=E(y)=E(Gx)=GE(x)=Gu_x\)
\(\Sigma_{yy}=E((y-u_y)(y-u_y)^T)=GE((x-u_x)(x-u_x)^T)G^T=G\Sigma_{xx}G^T\)
- 高斯概率密度函数的归一化积：
K个高斯概率密度函数的归一化积仍然是高斯概率密度函数：
\(exp(-\frac {1}{2}(x-u)^T\Sigma^{-1}(x-u))=\eta \prod\limits_{k=1}^{K}exp(-\frac {1}{2}(x-u_k)^T\Sigma_{k}^{-1}(x-u_k))\)
\(\Sigma^{-1}=\sum\limits_{i=1}^{n}\Sigma_k^{-1}\)
\(\Sigma^{-1}u=\sum\limits_{i=1}^{n}\Sigma_k^{-1}u_k\)
对于高斯分布随机变量的线性变换，可得到：
\(exp(-\frac {1}{2}(x-u)^T\Sigma^{-1}(x-u))=\eta \prod\limits_{k=1}^{K}exp(-\frac {1}{2}(G_kx-u_k)^T\Sigma_{k}^{-1}(G_kx-u_k))\)
\(\Sigma^{-1}=\sum\limits_{i=1}^{n}G_k^T\Sigma_k^{-1}G_k\)
\(\Sigma^{-1}u=\sum\limits_{i=1}^{n}G_k^T\Sigma_k^{-1}u_k\)
- 复合运算
考虑\(x\)和\(y\)，若不独立，复合分布为：\(p(x,y)=N(\left[ \begin{matrix} u_x \\ u_y \end{matrix} \right],\left[ \begin{matrix} \Sigma_{xx},\Sigma_{xy} \\ \Sigma_{yx},\Sigma_{yy} \end{matrix} \right] )\)，由条件分布\(P(x,y)=P(x|y)P(y)\)，条件概率满足：\(p(x|y)=N(u_x+\Sigma_{xy}\Sigma_{yy}^{-1}(y-u_y),\Sigma_{xx}-\Sigma_{xy}\Sigma_{yy}^{-1}\Sigma_{yx})\)
推导：
联合概率密度函数\(p(x|y)\)指数部分的二次项表示：
1.KF
针对线性高斯系统
系统运动方程：\(x_k=F_{k-1}x_{k-1}+G_{k-1}u_{k-1}+w_{k-1}, w_k\sim N(0,Q_k)\)
系统观测方程：\(y_k=H_kx_k+v_k,v_k\sim N(0,R_k)\)
贝叶斯滤波计算\(x_k\)的置信度：\(p(x_k|\breve x_0,u_{1:k},y_{0:k})=\eta p(y_k|x_k) \int p(x_k|x_{k-1},u_k)p(x_{k-1}|\breve x_0,u_{1:k-1},y_{0:k-1})dx_{k-1}\)
\(p(x_k|\breve x_0,u_{1:k},y_{0:k})\)后验置信度
\(p(y_k|x_k)\)利用h更新
\(p(x_k|x_{k-1},u_k)\)利用f预测
\(p(x_{k-1}|\breve x_0,u_{1:k-1},y_{0:k-1})\)先验置信度
- 预测部分：
设\(k-1\)时刻的高斯先验为：\(p(x_{k-1}|\breve x_0,u_{1:k-1},y_{0:k-1})=N(\hat x_{k-1},\hat P_{k-1})\)
随着\(k\)时刻的输入，计算\(k\)时刻的先验：
\(p(x_{k}|\breve x_0,u_{1:k},y_{0:k-1})=N(\breve x_{k},\breve P_{k})\)
\(\breve P_{k}=F_{k-1}\hat P_{k-1}F_{k-1}^T+Q_k\)
\(\breve x_{k}=F_{k-1}\hat x_{k-1}+G_{k-1}U_{k-1}\)
\(\breve x_{k}=E(x_k)=E(F_{k-1}x_{k-1}+G_{k-1}U_{k-1}+W_{k-1})=F_{k-1}\hat x_{k-1}+G_{k-1}U_{k-1}\)
\(\breve P_{k}=E[(x_k-E(x_k))(x_k-E(x_k))^T]\\=E[(F_{k-1}x_{k-1}+G_{k-1}U_{k-1}+W_{k-1}-F_{k-1}\hat x_{k-1}-G_{k-1}U_{k-1})()^T]\\=F_{k-1}E[(x_{k-1}-\hat x_{k-1})(x_{k-1}-\hat x_{k-1})^T]F_{k-1}^T+E[W_{k-1}W_{k-1}^T]\\=F_{k-1}\hat P_{k-1}F_{k-1}^T+Q_k\)
- 更新部分：
根据\(k\)时刻的观测数据，将状态与最近一次测量(即\(k\)时刻)写成联合高斯分布的形式：
\(p(x_{k},y_k|\breve x_0,u_{1:k},y_{0:k-1})=N(\left[ \begin{matrix} u_x \\ u_y \end{matrix} \right],\left[ \begin{matrix} \Sigma_{xx} & \Sigma_{xy} \\ \Sigma_{yx} & \Sigma_{yy} \end{matrix} \right])=N(\left[ \begin{matrix} \breve x_k \\ C_k\breve x_k \end{matrix} \right],\left[ \begin{matrix} \breve P_k & \breve P_kH_k^T \\ H_k\breve P_k & H_k\breve P_kH_k^T+R_k \end{matrix} \right])\)
\(P_{x_ky_k}=E[(x_k-E(x_k))(y_k-E(y_k))^T]\\=E[(F_{k-1}X_{k-1}+G_{k-1}U_{k-1}+W_{k-1}-F_{k-1}\hat x_{k-1}-G_{k-1}U_{k-1})(H_kX_k+V_k-H_k\hat x_k)^T)]\\=E[(F_{k-1}(x_{k-1}-\hat x_{k-1})+W_k)(H_kF_{k-1}(x_{k-1}-\hat x_{k-1})+H_kW_k+V_k)^T]\\=F_{k-1}E[(x_{k-1}-\hat x_{k-1})(x_{k-1}-\hat x_{k-1})^T]F_{k-1}^TH_k^T+E[W_kW_k^T]H_k^T\\=\breve P_kH_k^T\)
\(x_k\)的后验概率可写成：
\(p(x_k|\breve x_0,u_{1:k},y_{0:k})=N(u_x+\Sigma_{xy}\Sigma_{yy}^{-1}(y-u_y),\Sigma_{xx}-\Sigma_{xy}\Sigma_{yy}^{-1}\Sigma_{yx})\)
即：\(K_k=\breve P_kH_k^T(H_k\breve P_kH_k^T+R_k)^{-1}\)
\(\hat P_k=(I-K_kH_k)\breve P_{k}\)
\(\hat x_k=\breve x_{k}+K_k(y_k-H_k\breve x_{k})\)
可见：当观测噪声趋向于0时，\(K_k=H_k^{-1},x_k=H_k^{-1}y_k\)
当\(P_k\)趋向于0时，\(K_k=0,\hat x_k=\breve x_{k}\)
需要设置的参数：
初值\(P_0\)：表示对估计的\(x_0\)的不确定性，决定初始收敛速度。
建模噪声Q：与模型的线性化程度、离散化引入误差相关。
观测噪声R：与传感器特性有关。
2.EKF
针对非线性高斯系统
运动模型：\(x_k=f_{k-1}(x_{k-1},u_{k-1},w_{k-1}),w_k\sim N(0,Q_k)\)
观测模型：\(y_k=h_k(x_k,v_k),v_k\sim N(0,R_k)\)
采用线性化的方式泰勒展开：
\(x_k=f_{k-1}(x_{k-1},u_{k-1},w_{k-1})\\ \approx f_{k-1}(\hat x_{k-1},u_{k-1},0)+\left. \frac{ \partial f_{k-1}}{ \partial x_{k-1}} \right| _{\hat x_{k-1},u_{k-1},0}(x_{k-1}-\hat x_{k-1})+\left. \frac{ \partial f_{k-1}}{ \partial w_{k-1}} \right| _{\hat x_{k-1},u_{k-1},0}w_{k-1}\\ =f_{k-1}(\hat x_{k-1},u_{k-1},0)+F_k(x_{k-1}-\hat x_{k-1})+L_kw_{k-1}\)
\(y_k=h_k(x_k,v_k)\\ \approx h_k(\breve x_k,0)+\left. \frac{ \partial h_{k}}{ \partial x_{k}} \right| _{\breve x_{k},0}(x_{k}-\breve x_{k})+\left. \frac{ \partial h_{k}}{ \partial v_{k}} \right| _{\breve x_{k},0}v_{k}\\ =h_k(\breve x_k,0)+H_k(x_{k}-\breve x_{k})+M_kv_{k}\)
- 预测部分：
\(\breve x_{k}=f_{k-1}(\hat x_{k-1},u_{k-1},0)\)
\(\breve P_{k}=F\hat P_{k-1}F^T+LQ_kL^T\)
- 更新部分：
\(K_k=\breve P_kH_k^T(H_k\breve P_kH_k^T+M_kR_kM_k^T)^{-1}\)
\(\hat x_k=\breve x_{k}+K_k(y_k-h_k(\breve x_k,0))\)
\(\hat P_k=(I-K_kH_k)\breve P_{k}\)
3.ESKF
\(x_t=\hat x+\delta x\)，\(x_t\)是true state，\(\hat x\)是名义值nominal state， \(\delta x\)是error state。
滤波状态量是误差状态，用误差状态矫正名义状态。
- 运动模型：
\(x_k=f_{k-1}(x_{k-1},u_{k-1},w_{k-1}),w_k\sim N(0,Q_k)\)
\(x_k-f_{k-1}(\hat x_{k-1},u_{k-1},0)=F_k(x_{k-1}-\hat x_{k-1})+L_kw_{k}\)
\(\delta x_k=F_k \delta x_{k-1}+L_kw_{k}\)
- 测量模型
\(y_k=h_k(x_k,v_k),v_k\sim N(0,R_k)\)
\(y_k-h_k(\breve x_k,0)=H_k(x_{k}-\breve x_{k})+M_kv_{k}\)
\(y_k-h_k(\breve x_k,0)=H_k\delta x_{k}+M_kv_{k}\)
流程：
(1)更新名义状态
\(\breve x_{k}=f_{k-1}( x_{k-1},u_{k-1},0)\)，\(x_{k-1}\)可以是\(\hat x_{k-1}、\breve x_{k-1}\)
\(\breve P_{k}=F\hat P_{k-1}F^T+LQ_kL^T\)
(2)测量值available
\(K_k=\breve P_kH_k^T(H_k\breve P_kH_k^T+M_kR_kM_k^T)^{-1}\)
\(\delta \hat x_k=K_k(y_k-h_k(\breve x_k,0))\)
\(\hat P_k=(I-K_kH_k)\breve P_{k}\)
更新名义状态：\(\hat x_k=\breve x_{k}+\delta \hat x_k\)
浙公网安备 33010602011771号