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(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{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)推导

当运动方程或观测方程为非线性的时候，无法再利用之前所述的线性变化关系进行推导，常用的解决方法是进行线性化，把非线性方程一阶泰勒展开成线性。即：
运动方程: ${\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}$$

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

由于非线性模型中做了线性化近似，当非线性程度越强时，误差就会较大。但是，由于线性化的工作点离真值越近，线性化的误差就越小，因此解决该问题的一个方法是，通过迭代逐渐找到准确的线性化点，从而提高精度。
在EKF的推导中，其他保持不变，仅改变观测的线性化工作点，则有：
$$\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}$$按照与之前同样的方式进行推导，可得到滤波的校正过程为：
$$\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{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$ 更新的公式与之前有所不同。
滤波过程中，反复执行这 2 个公式，以上次的后验均值作为本次的线性化工作点，即可达到减小非线性误差的目的。
需要注意的是，在这种滤波模式下， 后验方差应放在最后一步进行。
$${\hat{\mathbf{P}}}_k=\left(1-{\mathbf{K}}_k{\mathbf{G}}_k\right){\check{\mathbf{P}}}_k$$

4. 基于滤波器的融合

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

4.1 状态方程

状态方程 $F$ 由误差方程得来，第8讲已经完成误差方程的推导：
$$\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 \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]$$初始方差理论上可设置为各变量噪声的平方，实际中一般故意设置大一些，这样可加快收敛速度。
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}$$按照之前讲解的惯性解算方法，进行位姿更新，该位姿属于先验位姿。
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$)。