多传感器融合定位十-基于滤波的融合方法Ⅰ其二

Reference:

1. 深蓝学院-多传感器融合
2. 多传感器融合定位理论基础

文章跳转：

1. 多传感器融合定位一-3D激光里程计其一：ICP
2. 多传感器融合定位二-3D激光里程计其二：NDT
3. 多传感器融合定位三-3D激光里程计其三：点云畸变补偿
4. 多传感器融合定位四-3D激光里程计其四：点云线面特征提取
5. 多传感器融合定位五-点云地图构建及定位
6. 多传感器融合定位六-惯性导航原理及误差分析
7. 多传感器融合定位七-惯性导航解算及误差分析其一
8. 多传感器融合定位八-惯性导航解算及误差分析其二
9. 多传感器融合定位九-基于滤波的融合方法Ⅰ其一
10. 多传感器融合定位十-基于滤波的融合方法Ⅰ其二
11. 多传感器融合定位十一-基于滤波的融合方法Ⅱ
12. 多传感器融合定位十二-基于图优化的建图方法其一
13. 多传感器融合定位十三-基于图优化的建图方法其二
14. 多传感器融合定位十四-基于图优化的定位方法
15. 多传感器融合定位十五-多传感器时空标定(综述)

3. 滤波器基本原理

3.1 状态估计模型

实际状态估计任务中，待估计的后验概率密度可以表示为：
$$p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k}\right)$$其中：
${\check{\mathbf{x}}}_0$ 表示的是状态初始值
${\mathbf{v}}_{1:k}$ 表示从 $1$ 到 $k$ 时刻的输入
${\mathbf{y}}_{0:k}$ 表示从 $0$ 到 $k$ 时刻的观测
因此，滤波问题可以直观表示为，根据所有历史数据(输入、观测、初始状态)得出最终的融合结果。

历史数据之间的关系，可以用下面的图模型表示，
图模型中体现了马尔可夫性，即当前状态只跟前一时刻状态相关，和其他历史时刻状态无关。
该性质的数学表达：
运动方程：${\mathbf{x}}_k=\mathbf{f}\left({\mathbf{x}}_{k-1},{\mathbf{v}}_k,{\mathbf{w}}_k\right)$
观测方程：${\mathbf{y}}_k=\mathbf{g}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)$

3.2 贝叶斯滤波

公式的推导可参考：非线性优化
根据贝叶斯公式，$k$ 时刻后验概率密度可以表示为：
$$\begin{array}{rl}p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k}\right)& =\frac{p\left({\mathbf{y}}_k\mid {\mathbf{x}}_k,{\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)}{p\left({\mathbf{y}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)}\\ & =\eta p\left({\mathbf{y}}_k\mid {\mathbf{x}}_k,{\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)\end{array}$$(这里 ${\mathbf{y}}_{\mathbf{k}}$ 是当前时刻的观测，而 $p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k}\right)$ 是当前时刻后验，$p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)$为先验。我们需要的是后验概率最大化，因为贝叶斯分母部分与待估计的状态无关，因而可以忽略)

根据观测方程，${\mathbf{y}}_k$ 只和 ${\mathbf{x}}_k$ 相关，因此上式可以简写为：
$$p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k}\right)=\eta p\left({\mathbf{y}}_k\mid {\mathbf{x}}_k\right)p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)$$利用条件分布的性质，可得：
$$\begin{array}{rl}& p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)\\ & =\int p\left({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1},{\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)p\left({\mathbf{x}}_{k-1}\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)\mathrm{d}{\mathbf{x}}_{k-1}\end{array}$$再利用马尔可夫性，可得：
$$\begin{array}{rl}& p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)\\ & =\int p\left({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1},{\mathbf{v}}_k\right)p\left({\mathbf{x}}_{k-1}\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)\mathrm{d}{\mathbf{x}}_{k-1}\end{array}$$经过以上化简，最终后验概率可以写为(观测+运动+前一帧结果)：

根据以上结果，可以画出贝叶斯滤波的信息流图如下：

贝叶斯滤波是一个非常广泛的概念，它不特指某一种滤波：

1. 在高斯假设前提下，用贝叶斯滤波的原始形式推导比较复杂，可以利用高斯的特征得到简化形式，即广义高斯滤波。后面 KF、EKF、IEKF 的推导均采用这种形式。
2. 实际中，UKF 和 PF 多应用于扫地机器人等2D小场景，与本课程目标场景不符，因此不做讲解。（UKF 和 PF 本身有一个维度的问题，维度高了不太行，而我们这里使用的维度多半是 $15$ 维的，在三维场景就不好用了）

3.3 卡尔曼滤波(KF)推导

之前写的卡尔曼文章可见此处跳转：滤波笔记一：卡尔曼滤波
在线性高斯假设下，上式可以重新写为下面的形式(为了和后面符号对应)
运动方程：${\mathbf{x}}_k=\mathbf{F}\left({\mathbf{x}}_{k-1},{\mathbf{v}}_k\right)+{\mathbf{B}}_{k-1}{\mathbf{w}}_k$
观测方程：${\mathbf{y}}_k=\mathbf{G}\left({\mathbf{x}}_k\right)+{\mathbf{C}}_k{\mathbf{n}}_k$
($\mathbf{F}$ 和 $\mathbf{G}$ 在这里代表的是线性的意思，非线性是后面要推导的。这里的 $\mathbf{G}$ 和之前卡尔曼文章里写的 $\mathbf{H}$ 是一个东西。$\mathbf{n}$ 为观测噪声，是个零均值白噪声)
把上一时刻的后验状态写为(因为后验本身是高斯形式，所以直接写成均值和方差的标准形式)：
$$p\left({\mathbf{x}}_{k-1}\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k-1},{\mathbf{y}}_{0:k-1}\right)=\mathcal{N}\left({\hat{\mathbf{x}}}_{k-1},{\hat{\mathbf{P}}}_{k-1}\right)$$则当前时刻的预测值为(根据运动方程推测的新时刻位置)：
$${\check{\mathbf{x}}}_k=\mathbf{F}\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{v}}_k\right)$$根据高斯分布的线性变化，它的方差为(可仿照第2.8节中的推导过程自行推导)(根据运动方程推测的新时刻方差)：
$${\check{\mathbf{P}}}_k={\mathbf{F}}_{k-1}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_{k-1}^{\mathrm{T}}+{\mathbf{B}}_{k-1}{\mathbf{Q}}_k{\mathbf{B}}_{k-1}^{\mathrm{T}}$$其中 $Q_k$ 为当前输入噪声的方差。

若把 $k$ 时刻状态和观测的联合高斯分布写为：
$$p\left({\mathbf{x}}_k,{\mathbf{y}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)=\mathcal{N}\left(\left[\begin{array}{c}{\mathbf{\mu }}_{x,k}\\ {\mathbf{\mu }}_{y,k}\end{array}\right],\left[\begin{array}{cc}{\mathbf{\Sigma }}_{xx,k}& {\mathbf{\Sigma }}_{xy,k}\\ {\mathbf{\Sigma }}_{yx,k}& {\mathbf{\Sigma }}_{yy,k}\end{array}\right]\right)$$根据第2.7节中的推导结果， $k$ 时刻的后验概率可以写为：
$$\begin{array}{rl}p& \left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k}\right)\\ & =\mathcal{N}(\underset{{\hat{\mathbf{x}}}_k}{\underbrace{{\mathbf{\mu }}_{x,k}+{\mathbf{\Sigma }}_{xy,k}{\mathbf{\Sigma }}_{yy,k}^{-1}\left({\mathbf{y}}_k-{\mathbf{\mu }}_{y,k}\right)}},\underset{{\hat{P}}_k}{\underbrace{{\mathbf{\Sigma }}_{xx,k}-{\mathbf{\Sigma }}_{xy,k}{\mathbf{\Sigma }}_{yy,k}^{-1}{\mathbf{\Sigma }}_{yx,k}}})\end{array}$$其中 ${\hat{\mathbf{x}}}_k$ 和 ${\hat{\mathbf{P}}}_k$ 分别为后验均值和方差。若定义：
$${\mathbf{K}}_k={\mathbf{\Sigma }}_{xy,k}{\mathbf{\Sigma }}_{yy,k}^{-1}$$则有：
$$\begin{array}{rl}& {\hat{\mathbf{P}}}_k={\check{\mathbf{P}}}_k-{\mathbf{K}}_k{\mathbf{\Sigma }}_{xy,k}^{\mathrm{T}}\\ & {\hat{\mathbf{x}}}_k={\check{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{y}}_k-{\mathbf{\mu }}_{y,k}\right)\end{array}$$把第2.8节中的推导得出的线性变换后的均值、方差及交叉项带入上面的式子，可以得到：
$$\begin{array}{rl}{\mathbf{K}}_k& ={\check{\mathbf{P}}}_k{\mathbf{G}}_k^{\mathrm{T}}{\left({\mathbf{G}}_k{\check{\mathbf{P}}}_k{\mathbf{G}}_k^{\mathrm{T}}+{\mathbf{C}}_k{\mathbf{R}}_k{\mathbf{C}}_k^{\mathrm{T}}\right)}^{-1}\\ {\hat{\mathbf{P}}}_k& =\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{G}}_k\right){\check{\mathbf{P}}}_k\\ {\hat{\mathbf{x}}}_k& ={\check{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{y}}_k-\mathbf{G}\left({\check{\mathbf{x}}}_k\right)\right)\end{array}$$上面方程与之前所述预测方程(如下)，就构成了卡尔曼经典五个方程。
$$\begin{array}{c}{\check{\mathbf{x}}}_k=\mathbf{F}\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{v}}_k\right)\\ {\check{\mathbf{P}}}_k={\mathbf{F}}_{k-1}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_{k-1}^{\mathrm{T}}+{\mathbf{B}}_{k-1}{\mathbf{Q}}_k{\mathbf{B}}_{k-1}^{\mathrm{T}}\end{array}$$需要说明的是，若不把第2.8节中的结果带入，而保留上页的原始形式，则对应的五个方程被称为广义高斯滤波。

3.4 扩展卡尔曼滤波(EKF)推导

之前写的扩展卡尔曼文章可见此处跳转：滤波笔记四：扩展卡尔曼滤波
当运动方程或观测方程为非线性的时候，无法再利用之前所述的线性变化关系进行推导，常用的解决方法是进行线性化，把非线性方程一阶泰勒展开成线性(在线性化点 $x_{op}$ 附近，所有的值都会用 $f(x)=f(x_{op})+F(x-x_{op})$ 表示)。即：
运动方程：${\mathbf{x}}_k=\mathbf{f}\left({\mathbf{x}}_{k-1},{\mathbf{v}}_k,{\mathbf{w}}_k\right)\approx {\check{\mathbf{x}}}_k+{\mathbf{F}}_{k-1}\left({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1}\right)+{\mathbf{B}}_{k-1}{\mathbf{w}}_k$
观测方程：${\mathbf{y}}_k=\mathbf{g}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)\approx {\check{\mathbf{y}}}_k+{\mathbf{G}}_k\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right)+{\mathbf{C}}_k{\mathbf{n}}_k$
其中：
$$\begin{array}{ll}{\check{\mathbf{x}}}_k=\mathbf{f}\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{v}}_k,0\right)& {\check{\mathbf{y}}}_k=\mathbf{g}\left({\check{\mathbf{x}}}_k,0\right)\\ {\mathbf{F}}_{k-1}={\frac{\partial \mathbf{f}\left({\mathbf{x}}_{k-1},{\mathbf{v}}_k,{\mathbf{w}}_k\right)}{\partial {\mathbf{x}}_{k-1}}|}_{{\hat{\mathbf{x}}}_{k-1},{\mathbf{v}}_k,0}& {\mathbf{G}}_k={\frac{\partial \mathbf{g}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)}{\partial {\mathbf{x}}_k}|}_{{\check{\mathbf{x}}}_k,0}\\ {\mathbf{B}}_{k-1}={\frac{\partial \mathbf{f}\left({\mathbf{x}}_{k-1},{\mathbf{v}}_k,{\mathbf{w}}_k\right)}{\partial {\mathbf{w}}_k}|}_{{\hat{\mathbf{x}}}_{k-1},{\mathbf{v}}_k,0}& {\mathbf{C}}_k={\frac{\partial \mathbf{g}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)}{\partial {\mathbf{n}}_k}|}_{{\check{\mathbf{x}}}_k,0}\end{array}$$根据该线性化展开结果，可以得到预测状态的统计学特征为：
$$\begin{array}{rl}& E\left[{\mathbf{x}}_k\right]\approx {\check{\mathbf{x}}}_k+{\mathbf{F}}_{k-1}\left({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1}\right)+\underset{0}{\underbrace{E\left[{\mathbf{B}}_{k-1}{\mathbf{w}}_k\right]}}\\ & E\left[\left({\mathbf{x}}_k-E\left[{\mathbf{x}}_k\right]\right){\left({\mathbf{x}}_k-E\left[{\mathbf{x}}_k\right]\right)}^{\mathrm{T}}\right]\approx \underset{{\mathbf{B}}_{k-1}{\mathbf{Q}}_k{\mathbf{B}}_{k-1}^{\mathrm{T}}}{\underbrace{E\left[{\mathbf{B}}_{k-1}{\mathbf{w}}_k{\mathbf{w}}_{\mathrm{T}}^{\mathrm{T}}{\mathbf{B}}_{k-1}^{\mathrm{T}}\right]}}\end{array}$$即 $p\left({\mathbf{x}}_k\mid {\mathbf{x}}_{k-1},{\mathbf{v}}_k\right)\approx \mathcal{N}\left({\check{\mathbf{x}}}_k+{\mathbf{F}}_{k-1}\left({\mathbf{x}}_{k-1}-{\hat{\mathbf{x}}}_{k-1}\right),{\mathbf{B}}_{k-1}{\mathbf{Q}}_k{\mathbf{B}}_{k-1}^{\mathrm{T}}\right)$
同理，可得到观测的统计学特征为：
$$\begin{array}{rl}& E\left[{\mathbf{y}}_k\right]\approx {\check{\mathbf{y}}}_k+{\mathbf{G}}_k\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right)+\underset{0}{\underbrace{E\left[{\mathbf{C}}_k{\mathbf{n}}_k\right]}}\\ & E\left[\left({\mathbf{y}}_k-E\left[{\mathbf{y}}_k\right]\right){\left({\mathbf{y}}_k-E\left[{\mathbf{y}}_k\right]\right)}^{\mathrm{T}}\right]\approx \underset{C_k{\mathbf{R}}_k{\mathbf{C}}_k^{\mathrm{T}}}{\underbrace{E\left[{\mathbf{C}}_k{\mathbf{n}}_k{\mathbf{n}}_k^{\mathrm{T}}{\mathbf{C}}_k^{\mathrm{T}}\right]}}\end{array}$$即 $p\left({\mathbf{y}}_k\mid {\mathbf{x}}_k\right)\approx \mathcal{N}\left({\check{\mathbf{y}}}_k+{\mathbf{G}}_k\left({\mathbf{x}}_k-{\check{\mathbf{x}}}_k\right),{\mathbf{C}}_k{\mathbf{R}}_k{\mathbf{C}}_k^{\mathrm{T}}\right)$

把均值和方差的具体形式，带入广义高斯滤波的公式，最终得到 EKF 下得经典五个公式。
$$\begin{array}{rl}& {\check{\mathbf{P}}}_k={\mathbf{F}}_{k-1}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_{k-1}^{\mathrm{T}}+{\mathbf{B}}_{k-1}{\mathbf{Q}}_k{\mathbf{B}}_{k-1}^{\mathrm{T}}\\ & {\check{\mathbf{x}}}_k=\mathbf{f}\left({\hat{\mathbf{x}}}_{k-1},{\mathbf{v}}_k,0\right)\\ & {\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{G}}_k^{\mathrm{T}}{\left({\mathbf{G}}_k{\check{\mathbf{P}}}_k{\mathbf{G}}_k^{\mathrm{T}}+{\mathbf{C}}_k{\mathbf{R}}_k{\mathbf{C}}_k^{\mathrm{T}}\right)}^{-1}\\ & {\hat{\mathbf{P}}}_k=\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{G}}_k\right){\check{\mathbf{P}}}_k\\ & {\hat{\mathbf{x}}}_k={\check{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{y}}_k-\mathbf{g}\left({\check{\mathbf{x}}}_k,0\right)\right)\end{array}$$

这里的 $f$ 和 $g$ 都不见得是线性的，如预测的位置和速度映射到极坐标上的距离(range)，方向(bearing)和距离变化率(range rate)：
$$g\left(x^{\prime }\right)=\left(\begin{array}{c}\rho \\ \varphi \\ \dot{\rho }\end{array}\right)=\left(\begin{array}{c}\sqrt{p_x^{\prime 2}+p_y^{\prime 2}}\\ \arctan \left(p_y^{\prime }/p_x^{\prime }\right)\\ \frac{p_x^{\prime }{v}_x^{\prime }+p_y^{\prime }{v}_y^{\prime }}{\sqrt{p_x^{\prime 2}+p_y^{\prime 2}}}\end{array}\right)$$

3.5 迭代扩展卡尔曼滤波(IEKF)推导

由于非线性模型中做了线性化近似，当非线性程度越强时，误差就会较大。但是，由于线性化的工作点离真值越近，线性化的误差就越小，因此解决该问题的一个方法是，通过迭代逐渐找到准确的线性化点，从而提高精度。
在EKF的推导中，其他保持不变，仅改变观测的线性化工作点，则有(在EKF中选取的是 $\mathbf{x}$ 的预测值 $\check{\mathbf{x}}$ 做的线性化，而这里是找的一个线性化的点，比前面的这个 ${\mathbf{x}}_{op}$ 更准)：
$$\mathbf{g}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)\approx {\mathbf{y}}_{\mathrm{op},k}+{\mathbf{G}}_k\left({\mathbf{x}}_k-{\mathbf{x}}_{\mathrm{op},k}\right)+{\mathbf{C}}_k{\mathbf{n}}_k$$其中：
$$\begin{array}{rl}& {\mathbf{y}}_{\mathrm{op},k}=\mathbf{g}\left({\mathbf{x}}_{\mathrm{op},k},0\right)\\ & {\mathbf{G}}_k={\frac{\partial \mathbf{g}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)}{\partial {\mathbf{x}}_k}|}_{{\mathbf{x}}_{\mathrm{op},k},0}\\ & {\mathbf{C}}_k={\frac{\partial \mathbf{g}\left({\mathbf{x}}_k,{\mathbf{n}}_k\right)}{\partial {\mathbf{n}}_k}|}_{{\mathbf{x}}_{\mathrm{op},\mathbf{k}},0}\end{array}$$按照与之前同样的方式进行推导，可得到滤波的校正过程为(这里的解决方法就是，先计算一个 $\mathbf{x}$ 的后验 $\hat{\mathbf{x}}$，这里的后验肯定比先验要准，那么有了这个更准的值，再做一次卡尔曼滤波，这时线性化点就更准了)：
$$\begin{array}{l}{\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{G}}_k^{\mathrm{T}}{\left({\mathbf{G}}_k{\check{\mathbf{P}}}_k{\mathbf{G}}_k^{\mathrm{T}}+{\mathbf{C}}_k{\mathbf{R}}_k{\mathbf{C}}_k^{\mathrm{T}}\right)}^{-1}\\ {\hat{\mathbf{P}}}_k=\left(1-{\mathbf{K}}_k{\mathbf{G}}_k\right){\check{\mathbf{P}}}_k\\ {\hat{\mathbf{x}}}_k={\check{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{y}}_k-{\mathbf{y}}_{\mathrm{op},k}-{\mathbf{G}}_k\left({\check{\mathbf{x}}}_k-{\mathbf{x}}_{\mathrm{op},k}\right)\right)\end{array}$$可见唯一的区别是后验均值 ${\hat{\mathbf{x}}}_k$ 更新的公式与之前有所不同，五个步骤仅迭代 ${\hat{\mathbf{x}}}_k={\check{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{y}}_k-{\mathbf{y}}_{\mathrm{op},k}-{\mathbf{G}}_k\left({\check{\mathbf{x}}}_k-{\mathbf{x}}_{\mathrm{op},k}\right)\right)$ 就够了。
滤波过程中，反复执行第3个公式，以上次的后验均值作为本次的线性化工作点，即可达到减小非线性误差的目的。当达到终止条件时，再执行一次前两个公式。

什么时候结束：如果这一次迭代和上一次迭代相比，差异很小，就可以结束了。

4. 基于滤波器的融合

通过以上推导，滤波问题可以简单理解为“预测 + 观测 = 融合结果”。
结合实际点云地图中定位的例子，预测由IMU给出，观测即为激光雷达点云和地图匹配得到的姿态和位置。
融合流程用框图可以表示如下：

IMU 有累积误差，这里的修正误差步骤可以用来消一下。

4.1 状态方程

状态方程 $F$ 由误差方程得来，第8讲已经完成误差方程的推导(这里使用的方法是 误差状态卡尔曼(ESKF)，虽然我们最终要输出的是 位置、速度、姿态 等等东西，但是我们关心的是它们的误差量，我们以误差量作为状态量来进行卡尔曼滤波。所以这里我们的状态量就是所有这些东西的误差量)：
$$\begin{array}{rl}& \delta \dot{\mathbf{p}}=\delta \mathbf{v}\\ & \delta \dot{\mathbf{v}}=-{\mathbf{R}}_t{\left[{\mathbf{a}}_t-{\mathbf{b}}_{a_t}\right]}_{\times }\delta \mathbf{\theta }+{\mathbf{R}}_t\left({\mathbf{n}}_a-\delta {\mathbf{b}}_a\right)\\ & \delta \dot{\mathbf{\theta }}=-{\left[{\mathbf{\omega }}_t-{\mathbf{b}}_{{\omega }_t}\right]}_{\times }\delta \mathbf{\theta }+{\mathbf{n}}_{\omega }-\delta {\mathbf{b}}_{\omega }\\ & \delta {\dot{\mathbf{b}}}_a={\mathbf{n}}_{b_a}\text{ 或 }\delta {\dot{\mathbf{b}}}_a=0\\ & \delta {\dot{\mathbf{b}}}_{\omega }={\mathbf{n}}_{b_{\omega }}{}^{}{}^{}{}^{}{}^{}{}^{}{}^{}\delta {\dot{\mathbf{b}}}_{\omega }=0\\ & \text{ 令 }\delta \mathbf{x}=\left[\begin{array}{c}\delta \mathbf{p}\\ \delta \mathbf{v}\\ \delta \mathbf{\theta }\\ \delta {\mathbf{b}}_a\\ \delta {\mathbf{b}}_{\omega }\end{array}\right],\mathbf{w}=\left[\begin{array}{l}{\mathbf{n}}_a\\ {\mathbf{n}}_{\omega }\\ {\mathbf{n}}_{b_a}\\ {\mathbf{n}}_{b_{\omega }}\end{array}\right]\end{array}$$有了误差量 $\delta \mathbf{x}$ 及它们之间关系，那么写 $\mathbf{F}$ 和 $\mathbf{B}$ 就很容易了。
则误差方程可以写成状态方程的通用形式：
$$\delta \dot{\mathbf{x}}={\mathbf{F}}_t\delta \mathbf{x}+{\mathbf{B}}_t\mathbf{w}$$其中(这里直接将上面公式写成矩阵形式，不需要做任何改变)：
$$\begin{array}{rl}{\mathbf{F}}_t& =\left[\begin{array}{ccccc}0& {\mathbf{I}}_3& 0& 0& 0\\ 0& 0& -{\mathbf{R}}_t{\left[{\overline{\mathbf{a}}}_t\right]}_{\times }& -{\mathbf{R}}_t& 0\\ 0& 0& -{\left[{\overline{\mathbf{\omega }}}_t\right]}_{\times }& 0& -{\mathbf{I}}_3\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right]\begin{array}{c}{\overline{\mathbf{a}}}_t={\mathbf{a}}_t-{\mathbf{b}}_{a_t}\\ {\overline{\mathbf{\omega }}}_t={\mathbf{\omega }}_t-{\mathbf{b}}_{{\omega }_t}\end{array}\\ {\mathbf{B}}_t& =\left[\begin{array}{cccc}0& 0& 0& 0\\ {\mathbf{R}}_t& 0& 0& 0\\ 0& {\mathbf{I}}_3& 0& 0\\ 0& 0& {\mathbf{I}}_3& 0\\ 0& 0& 0& {\mathbf{I}}_3\end{array}\right]\end{array}$$注：当选择 $\delta {\dot{\mathbf{b}}}_a=0$，$\delta {\dot{\mathbf{b}}}_{\omega }=0$ 时，矩阵形式不一样，请各位自行推导。

4.2 观测方程

在滤波器中，观测方程 $G$ 一般写为：
$$\mathbf{y}={\mathbf{G}}_t\delta \mathbf{x}+{\mathbf{C}}_t\mathbf{n}$$此例中观测量有位置、失准角，则(对于雷达和地图匹配，匹配完之后得到的是一个【位置+姿态/平移+旋转】，对于状态量来讲，一个观测对应两个状态量----平移和旋转的误差量。这里就需要将平移和旋转的误差量，变成想要的观测值)：
$$\mathbf{y}=\left[\begin{array}{l}\delta \overline{\mathbf{p}}\\ \delta \overline{\mathbf{\theta }}\end{array}\right]$$因此有：
$$\begin{array}{rl}{\mathbf{G}}_t& =\left[\begin{array}{ccccc}{\mathbf{I}}_3& 0& 0& 0& 0\\ 0& 0& {\mathbf{I}}_3& 0& 0\end{array}\right]\\ {\mathbf{C}}_t& =\left[\begin{array}{cc}{\mathbf{I}}_3& 0\\ 0& {\mathbf{I}}_3\end{array}\right]\end{array}$$而此处 $n$ 为观测噪声，
$$\mathbf{n}={\left[\begin{array}{llllll}{n}_{\delta {\bar{p}}_x}& n_{\delta {\bar{p}}_y}& n_{\delta {\bar{p}}_z}& n_{\delta {\bar{\theta }}_x}& n_{\delta {\bar{\theta }}_y}& n_{\delta {\bar{\theta }}_z}\end{array}\right]}^T$$观测量中， $\delta \mathbf{p}$ 的计算过程为(平移误差量的求法)：
$$\delta \overline{\mathbf{p}}=\check{\mathbf{p}}-\mathbf{p}$$其中 $\check{\mathbf{p}}$ 为 IMU 解算的位置，即预测值。$\mathbf{p}$ 为雷达与地图匹配得到的位置，即观测值。
$\delta \overline{\mathbf{\theta }}$ 的计算过程稍微复杂，需要先计算误差矩阵，
$$\delta {\overline{\mathbf{R}}}_t={\mathbf{R}}_t^T{\check{\mathbf{R}}}_t$$其中 ${\check{\mathbf{R}}}_t$ 为 IMU 解算的旋转矩阵，即预测值。${\mathbf{R}}_t$ 为雷达与地图匹配得到的旋转矩阵，即观测值。
由于预测值与观测值之间的关系为：
$${\check{\mathbf{R}}}_t\approx {\mathbf{R}}_t\left(\mathbf{I}+[\delta \overline{\mathbf{\theta }}{]}_{\times }\right)$$因此：
$$\delta \overline{\mathbf{\theta }}={\left(\delta {\overline{\mathbf{R}}}_t-\mathbf{I}\right)}^{\vee }$$

4.3 构建滤波器

构建滤波器，即把融合系统的状态方程和观测方程应用到 Kalman 滤波的五个公式中。
前面推导的方程是连续时间的，要应用于离散时间，需要按照采样时间对其进行离散化。
状态方程离散化，可以写为：
$$\delta {\mathbf{x}}_k={\mathbf{F}}_{k-1}\delta {\mathbf{x}}_{k-1}+{\mathbf{B}}_{k-1}{\mathbf{w}}_k$$其中：
$$\begin{array}{rl}{\mathbf{F}}_{k-1}& ={\mathbf{I}}_{15}+{\mathbf{F}}_{t}T\\ {\mathbf{B}}_{k-1}=& \left[\begin{array}{cccc}0& 0& 0& 0\\ {\mathbf{R}}_{k-1}T& 0& 0& 0\\ 0& {\mathbf{I}}_{3}T& 0& 0\\ 0& 0& {\mathbf{I}}_3\sqrt{T}& 0\\ 0& 0& 0& {\mathbf{I}}_3\sqrt{T}\end{array}\right]\end{array}$$其中，$T$ 为 Kalman 的滤波周期。
注：关于 ${\mathbf{B}}_{k-1}$ 的离散化形式，不同资料有差异，但对实际调试影响不大。
对于观测方程，不需要乘以滤波周期，可以直接写出：
$${\mathbf{y}}_k={\mathbf{G}}_k\delta {\mathbf{x}}_k+{\mathbf{C}}_k{\mathbf{n}}_k$$将以上各变量，带入kalman滤波的五个方程，即可构建完整的滤波器更新流程。
$$\begin{array}{rl}& \delta {\check{\mathbf{x}}}_k={\mathbf{F}}_{k-1}\delta {\hat{\mathbf{x}}}_{k-1}+{\mathbf{B}}_{k-1}{\mathbf{w}}_k\\ & {\check{\mathbf{P}}}_k={\mathbf{F}}_{k-1}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_{k-1}^{\mathrm{T}}+{\mathbf{B}}_{k-1}{\mathbf{Q}}_k{\mathbf{B}}_{k-1}^T\\ & {\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{G}}_k^{\mathrm{T}}{\left({\mathbf{G}}_k{\check{\mathbf{P}}}_k{\mathbf{G}}_k^{\mathrm{T}}+{\mathbf{C}}_k{\mathbf{R}}_k{\mathbf{C}}_k^T\right)}^{-1}\\ & {\hat{\mathbf{P}}}_k=\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{G}}_k\right){\check{\mathbf{P}}}_k\\ & \delta {\hat{\mathbf{x}}}_k=\delta {\check{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{y}}_k-{\mathbf{G}}_k\delta {\check{\mathbf{x}}}_k\right)\end{array}$$

4.4 Kalman 滤波实际使用流程

4.4.1 位姿初始化

在点云地图中实现初始定位，并给以下变量赋值，
${\hat{\mathbf{p}}}_0$ ：初始时刻位置
${\hat{\mathbf{v}}}_0$ ：初始时刻速度(可以从组合导航获得)
${\hat{\mathbf{R}}}_0$：初始时刻姿态(也可用四元数，后面不再强调)

4.4.2 Kalman 初始化

a. 状态量 $\delta {\hat{\mathbf{x}}}_0=0$
b. 方差
$${\hat{\mathbf{P}}}_0=\left[\begin{array}{ccccc}{\mathbf{P}}_{\delta p}& & & & \\ & {\mathbf{P}}_{\delta v}& & & \\ & & {\mathbf{P}}_{\delta \mathbf{\theta }}& & \\ & & & {\mathbf{P}}_{\delta b_a}& \\ & & & & {\mathbf{P}}_{\delta b_{\omega }}\end{array}\right]$$初始方差理论上可设置为各变量噪声的平方，实际中一般故意设置大一些，这样可加快收敛速度-----初始方差越大，观测在前几步起的作用就越大，那么收敛就越快。(比如如果认为位置上有 $0.1$ 的误差，那么方差应该给 $0.01$)
c. 过程噪声与观测噪声
$$\mathbf{Q}=\left[\begin{array}{cccc}{\mathbf{Q}}_a& & & \\ & {\mathbf{Q}}_{\omega }& & \\ & & {\mathbf{Q}}_{b_a}& \\ & & & {\mathbf{Q}}_{b_{\omega }}\end{array}\right]{\mathbf{R}}_0=\left[\begin{array}{ll}{\mathbf{R}}_{\delta p}& \\ & {\mathbf{R}}_{\delta \theta }\end{array}\right]$$过程噪声与观测噪声一般在 kalman 迭代过程中保持不变(一般噪声的平方项是这里的方差，但是实际上还是要调参)。

4.4.3 惯性解算

按照之前讲解的惯性解算方法，进行位姿更新，该位姿属于先验位姿。
a. 姿态解算：
$${\check{\mathbf{R}}}_k={\hat{\mathbf{R}}}_{k-1}\left(I+\frac{\sin \varphi }{\varphi }(\varphi \times )+\frac{1-\cos \varphi }{{\varphi }^2}(\mathbf{\varphi }\times {)}^2\right)$$其中：
$$\begin{array}{rl}& \mathbf{\varphi }=\frac{{\overline{\mathbf{\omega }}}_{k-1}+{\overline{\mathbf{\omega }}}_k}{2}\left(t_k-t_{k-1}\right)\\ & {\overline{\mathbf{\omega }}}_k={\mathbf{\omega }}_k-{\mathbf{b}}_{{\omega }_k}\\ & {\overline{\mathbf{\omega }}}_{k-1}={\mathbf{\omega }}_{k-1}-{\mathbf{b}}_{{\omega }_{k-1}}\end{array}$$之前惯性解算的时候的 $\mathbf{\omega }$ 是一个理想状态下的 $\mathbf{\omega }$，没有考虑各种误差，也就是 bias 的问题，在这里 bias 都作为误差量了，必然是要将 bias 考虑进来的，所以这里的 $\mathbf{\omega }$ 都是要先减去 bias 的。

按照之前讲解的惯性解算方法，进行位姿更新，该位姿属于先验位姿。
b. 速度解算：
$${\check{\mathbf{v}}}_k={\hat{\mathbf{v}}}_{k-1}+\left(\frac{{\check{\mathbf{R}}}_k{\overline{\mathbf{a}}}_k+{\hat{\mathbf{R}}}_{k-1}{\overline{\mathbf{a}}}_{k-1}}{2}-\mathbf{g}\right)\left(t_k-t_{k-1}\right)$$其中：
$$\begin{array}{rl}& {\overline{\mathbf{a}}}_k={\mathbf{a}}_k-{\mathbf{b}}_{a_k}\\ & {\overline{\mathbf{a}}}_{k-1}={\mathbf{a}}_{k-1}-{\mathbf{b}}_{a_{k-1}}\end{array}$$c. 位置解算：
$${\hat{\mathbf{p}}}_k={\check{\mathbf{p}}}_{k-1}+\frac{{\check{\mathbf{v}}}_k+{\hat{\mathbf{v}}}_{k-1}}{2}\left(t_k-t_{k-1}\right)$$

4.4.4 Kalman 预测更新

执行kalman五个步骤中的前两步，即：
$$\begin{array}{rl}& \delta {\check{\mathbf{x}}}_k={\mathbf{F}}_{k-1}\delta {\hat{\mathbf{x}}}_{k-1}+{\mathbf{B}}_{k-1}{\mathbf{w}}_k\\ & {\check{\mathbf{P}}}_k={\mathbf{F}}_{k-1}{\hat{\mathbf{P}}}_{k-1}{\mathbf{F}}_{k-1}^{\mathrm{T}}+{\mathbf{B}}_{k-1}{\mathbf{Q}}_k{\mathbf{B}}_{k-1}^T\end{array}$$当然，这需要先根据公式计算 ${\mathbf{F}}_{k-1}$ 和 ${\mathbf{B}}_{k-1}$ 。

4.4.5 无观测时后验更新

无观测时(无雷达匹配结果)，不需要执行kalman剩下的三个步骤，后验等于先验，即：
$$\begin{array}{rl}& \delta {\hat{\mathbf{x}}}_k=\delta {\check{\mathbf{x}}}_k\\ & {\hat{\mathbf{P}}}_k={\check{\mathbf{P}}}_k\\ & {\hat{\mathbf{x}}}_k={\check{\mathbf{x}}}_k\end{array}$$

4.4.6 有观测时的量测更新

执行kalman滤波后面的三个步骤，得到后验状态量。
$$\begin{array}{rl}& {\mathbf{K}}_k={\check{\mathbf{P}}}_k{\mathbf{G}}_k^{\mathrm{T}}{\left({\mathbf{G}}_k{\check{\mathbf{P}}}_k{\mathbf{G}}_k^{\mathrm{T}}+{\mathbf{C}}_k{\mathbf{R}}_k{\mathbf{C}}_k^T\right)}^{-1}\\ & {\hat{\mathbf{P}}}_k=\left(\mathbf{I}-{\mathbf{K}}_k{\mathbf{G}}_k\right){\check{\mathbf{P}}}_k\\ & \delta {\hat{\mathbf{x}}}_k=\delta {\check{\mathbf{x}}}_k+{\mathbf{K}}_k\left({\mathbf{y}}_k-{\mathbf{G}}_k\delta {\check{\mathbf{x}}}_k\right)\end{array}$$

4.4.7 有观测时计算后验位姿

根据后验状态量，更新后验位姿。
$$\begin{array}{rl}& {\hat{\mathbf{p}}}_k={\check{\mathbf{p}}}_k-\delta {\hat{\mathbf{p}}}_k\\ & {\hat{\mathbf{v}}}_k={\check{\mathbf{v}}}_k-\delta {\hat{\mathbf{v}}}_k\\ & {\hat{\mathbf{R}}}_k={\check{\mathbf{R}}}_k\left(\mathbf{I}-{\left[\delta {\hat{\mathbf{\theta }}}_k\right]}_{\times }\right)\\ & {\hat{\mathbf{b}}}_{a_k}={\check{\mathbf{b}}}_{a_k}-\delta {\hat{\mathbf{b}}}_{a_k}\\ & {\hat{\mathbf{b}}}_{{\omega }_k}={\check{\mathbf{b}}}_{{\omega }_k}-\delta {\hat{\mathbf{b}}}_{{\omega }_k}\end{array}$$

4.4.8 有观测时状态量清零

状态量已经用来补偿，因此需要清零。
$$\delta {\hat{\mathbf{x}}}_k=0$$后验方差保持不变。

4.4.9 输出位姿

把后验位姿输出给其他模块使用，即输出 ${\hat{\mathbf{p}}}_k$，${\hat{\mathbf{v}}}_k$，${\hat{\mathbf{R}}}_k$ (或 ${\hat{\mathbf{q}}}_k$)。