多传感器融合中的最小二乘

0. 前言

从接触SLAM开始，多传感器融合一直是该领域的核心，自己也对这块充满了好奇。下面就讲一个自己最开始接触传感器融合的故事吧。

1. 导入
   在大学期间，第一次做平衡车开始，需要让小车保持平衡，就需要对平衡车进行倾斜角的测量，用到了常见的MEMS惯性传感器MPU6050，里面有XYZ三轴的加速度计，还有XYZ三轴的陀螺仪也就是角速度计，按照常理而言，直接对各个轴的角速度进行积分可以计算出倾角，但是由于设备低廉，你不能保证它静止的时候角速度测量值真的为0度/s，直接积分容易出问题。
2. 转机
   于是各种查资料，得知物体一直受到重力影响，而加速度刚好又是专门测量加速度的。那么就开始了假设，物体在匀速运动过程中，通过倾斜过程中测量得到的重力加速度在各个轴的分量，可以计算出倾角的大小【狂喜】。实现的过程中发现，物体并不是永远匀速只受到重力加速度，小车稍微的电机振动，就偏差大的不得了。
3. 加权融合
   加速度测量得到的角度有部分真实可信，但是角速度直接积分又容易出问题。于是又查资料得知，在每次角速度积分的过程中，将加速度测量的角度进行加权，作为本次积分的最终结果，发现效果还不错。于是小车的倾角测量也就终于解决了，后面就是如何根据倾角控制小车平衡了。
4. 心得
   说实话，不断的查资料解决各种问题让我很有成就感，然后我也就慢慢接触融合相关的内容了。最后，不断的有中想法，就是有没有更好的方案，后面了解到了卡尔曼滤波，然后就是EKF、UKF、ESKF、IEKF。截至目前要讲的非线性优化融合。但是需要注意的是，截至到下面介绍非线性的方案之前，困扰我的是如何使得迭代更加准确，然而融合优化的过程很多时候不一定对实时有那么高的要求，而是对全局的误差最小。

1. 非线性优化融合简介

1. 这套理论是基于最小二乘法的，里面的有些相关内容与EKF很相似，但又有些思想不太一样。EKF是将非线性测量系统进行了一阶泰勒展开，然后根据变换后的观测先验与真实观测的残差进行状态的优化，需要自己看完这方面后自己总结。
2. 下面这方面的内容，假设你已经对最小二乘的拟合、求解已经有了初步的认识。
3. 下面我是基于 IMU 与 Camera 来讲述的。我们将从一个从顶向下的逻辑认识，也就是由简单到复杂的过程。

2. 基于最小二乘法的多传感器融合步骤

2.1 优化的对象：全局 or 局部

进行优化之前，你需要明确你是要全局整体误差最小，还是考虑要求实时性较高的局部最优，这二者的思想有些不太一样。，这篇文章主要对全局优化进行介绍。

• 将全局的观测数据进行最优化：这个一般会对 所有的边和节点，结合所有观测的误差信息，整体计算一次，得到 每一个节点的状态 的最优状态。这个的实现有两种，一种是每新增一个数据就重新计算。另一种就是每新增一个数据，内部自动保留以前的结果，增量更新，就不用手动添加全部节点，只需要增量假如新节点，这个也会更新全部节点，如ISAM2，适合实时性较高的场景。两种的优化结果理论上一致，能够使得全局优化保持一致。

• 具有先验信息的局部最优化：这种一般会将以前的优化信息 总结为先验信息，包括先验状态、先验协方差等信息，然后结合当前的测量信息以及观测信息，进行后验估计，包含后验状态、后验协方差。然后作为下一次迭代优化的先验信息。 这种是迭代优化方案，具有局部最优，无法保证全局最优。如EKF、ESKF、UKF等

2.2. 定义系统需要优化状态

• 在进行优化之前，我们必须知道系统需要最终优化的变量是什么，在SLAM系统中一般需要对机器人位置姿态进行计算。由于我们系统使用了IMU，那么再加一项状态就是关于IMU的测量偏差的估计 $bias$，最终系统的状态量如下，注意它是一个列向量
  $$x=[P_{wbj},Q_{wbj},V_j^w,b_a^{bj},b_g^{bj}{]}^T\text{\hspace{1em}( 式2-1)}$$

2.2 构造优化函数

• 与传统单项最小二乘法不同的是，多传感器融合涵盖不同传感器，这些传感器之间不会相互影响，因此传感器之间是不相关的，那么就可以单独考虑每个传感器单独构造的代价函数。
• 因为需要多传感器融合，而且最终的目的只有一个就是得到最优状态，那么就需要设计一个含全部信息的代价优化函数，也就是$F$，这个$F$需要包含IMU的信息，也要包含Camera的信息。实际上，这两个传感器，各自单独都可以估计我们最终的状态，那挺好，我们就根据他们各自独立的代价优化函数，最后给加起来就行。
• 假如IMU的独立代价优化函数为$F_{imu}$，相机的代价优化函数为$F_{cam}$，由此我们得到一个包含全部信息的代价优化函数 $F$
  $$\begin{array}{rl}F& =F_{imu}+F_{cam}\\ & =\sum _{i=1}^m||f_{imu}^i|{|}^2+\sum _{i=1}^n||f_{cam}^i|{|}^2\\ & =f_{imu}^T\cdot f_{imu}+f_{cam}^T\cdot f_{cam}\end{array}\text{\hspace{1em}( 式2-2)}$$
• 需要解释的是，$f_{imu}$是IMU构建的残差向量，具体表示如下，在优化的过程中，IMU的$i$到$j$时刻的预积分量 ${\alpha }_{b_i{b}_j}$、$q_{b_j{b}_i}$、$\beta b_j{b}_i$是已知的，而且上一时刻状态$p_{w{b}_i}$、$q_{b_{i}w}$、$v_i^w$、$b_i^a$、$b_i^g$也是已知的，剩下的也就是需要优化的状态量$x=[P_{wbj},Q_{wbj},V_j^w,b_a^{bj},b_g^{bj}]$是可变的，不过它是有初值的。【插一嘴】：残差向量函数中的残差项，实际上是由状态量进行变换后，与观测量如预积分之间的差值。这与EKF中的残差构建很像。
  $$f_{imu}=\left[\begin{array}{c}{f}_{imu}^1\\ \\ f_{imu}^2\\ \\ f_{imu}^3\\ \\ f_{imu}^4\\ \\ f_{imu}^5\end{array}\right]=\left[\begin{array}{c}{r}_p\\ \\ r_q\\ \\ r_v\\ \\ r_{ba}\\ \\ r_{bg}\end{array}\right]=\left[\begin{array}{c}{q}_{b_{i}w}(p_{w{b}_j}-p_{w{b}_i}-v_i^w△t+\frac{1}{2}{g}^w△t^2)-{\alpha }_{b_i{b}_j}\\ \\ 2[q_{b_j{b}_i}\otimes (q_{b_{i}w}\otimes q_{w{b}_j})]\\ \\ q_{b_{i}w}(v_j^w-v_i^w+g^w△t)-{\beta }_{b_i{b}_j}\\ \\ b_j^a-b_i^a\\ \\ b_j^g-b_i^g\end{array}\right]\text{\hspace{1em}(式2-3)}$$

2.3 状态变量的优化更新

• 根据代价函数的雅可比矩阵，来优化状态变量，已经有了相关算法研究，这里介绍最简单的最速梯度下降。需要注意$x_k$可以看作有值的。
  $$x_{k+1}-x_k=-J_k^T\text{\hspace{1em}(式2-4)}$$
• 本次更新中，每迭代更新都需要更新雅可比矩阵 $J_k$，这个$J_k$是总的代价优化函数F 关于 状态x 的雅可比矩阵，计算表达式如下，最终的结果为两部分独立代价函数对状态的雅可比矩阵之和。
  $$\begin{array}{rl}{J}_k& =\frac{\partial F}{\partial x}\\ & \\ & =\frac{\partial F_{imu}}{\partial x}+\frac{\partial F_{cam}}{\partial x}\\ & \\ & =[\frac{\partial F_{imu}}{\partial x_1},\frac{\partial F_{imu}}{\partial x_2}...\frac{\partial F_{imu}}{\partial x_n}]+[\frac{\partial F_{cam}}{\partial x_1},\frac{\partial F_{cam}}{\partial x_2}...\frac{\partial F_{cam}}{\partial x_n}]\end{array}\text{\hspace{1em}(式2-5)}$$
• 实际应用中，每一部分如 $\frac{\partial F_{imu}}{\partial x}$ 是相对计算的，如 LM优化中，它会先计算残差向量函数$f_{imu}$关于状态的雅可比矩阵$J_{f_{imu}}$，以及未更新状态前的残差向量值 $f_{imu}$，然后通过等价形式得到$F_{imu}$关于状态的雅可比矩阵。
  $$\frac{\partial F_{imu}}{\partial x}=(J_{f_{imu}}^T\cdot f_{imu}{)}^T，且J_{f_{imu}}^T=\frac{\partial f_{imu}}{\partial x}=\left[\begin{array}{c}{J}_{1}x\\ \\ J_{2}x\\ \\ ...\\ \\ J_{5}x\end{array}\right]\text{\hspace{1em}(式2-6)}$$

2.4 引入各自协方差作为更新权重

• 在因子图中，每一个因子的分布都是一个多维高斯分布，因此不同观测因子有着不同的协方差，那么该观测因子对于节点状态的更新权重不一样。此时需要考虑 IMU和Camera 各自协方差。
  $$p(x)=\frac{1}{\sqrt{|2\pi \Sigma |}}\cdot ex{p}^{-\frac{1}{2}(x-u{)}^T{\Sigma }^{-1}(x-u)}$$

• 信息矩阵：它是协方差矩阵的逆，内部的元素代表变量之间影响程度，0代表二者不影响，这玩意源自多元正态分布；协方差矩阵某元素为0，其对应信息矩阵元素不一定为0。对信息矩阵的元素进行操作时，需要了解边缘化、舒尔补。

• 假如IMU预积分量的协方差为 ${\Sigma }_{imu}$ ，在IMU的代价优化函数中，对于状态量引入信息矩阵的解为
  $$\underset{IMU预积分的贡献}{\underbrace{J_{f_{imu}}^T\cdot {\Sigma }_{imu}^{-1}\cdot J_{f_{imu}}}}\cdot △x=-\underset{IMU预积分的贡献}{\underbrace{J_{f_{imu}}^T\cdot {\Sigma }_{imu}^{-1}\cdot f_{imu}}}$$

• 对于多个因子的更新累加形式，自己推的话有些地方还是需要注意下，已核查的正确如下：
  $$（\sum _{i=1}{J}_{f_i}^T\cdot {\Sigma }_i^{-1}\cdot J_{f_i}）\cdot △x=（\sum J_{f_i}^T\cdot {\Sigma }_i^{-1}\cdot f_i）$$

3. IMU预积分相关知识

3.1 IMU预积分的残差

• 这部分内容实际上为了构建系统需要估计的状态与IMU预积分量之间的残差，实际上是为了服务状态估计的。只不过状态量为系统状态量，也就是待估计状态，然后预积分量作为观测，从而构建残差。
• IMU的残差构建如（式子2-3）

3.2 残差关于状态的雅可比矩阵

• 优化的过程中，待估计的状态 $x$ 一般是有初值的，变换后与观测量存在差值，此时需要通过差值更新状态 $x$，因此需要计算残差项关于状态的雅可比矩阵。

3.3 IMU预积分过程的协方差

• 在IMU进行预积分的过程中，加速度计和角速度计都是有噪声的，那么它的每一次积分都会使得误差累积，因此需要考量：1. 之前预积分的协方差如何变化，2. 当前积分过程中噪声对方差的变化.
• 噪声传播：因此期望得到下面在每次预积分迭代过程中，关于噪声的传播如下，$a$ 代表加速度计，$g$ 代表角速度计；$\delta$ 代表误差，$n$ 代表测量噪声。
  $$\left[\begin{array}{c}\delta {\alpha }_{k+1}\\ \\ \delta {\theta }_{k+1}\\ \\ \delta {\beta }_{k+1}\\ \\ \delta b_{a_{k+1}}\\ \\ \delta b_{g_{k+1}}\end{array}\right]=F\cdot \left[\begin{array}{c}\delta {\alpha }_k\\ \\ \delta {\theta }_k\\ \\ \delta {\beta }_k\\ \\ \delta b_{a_k}\\ \\ \delta b_{g_k}\end{array}\right]+G\cdot \left[\begin{array}{c}{n}_a\\ \\ n_g\\ \\ n_a\\ \\ n_{b_a}\\ \\ n_{b_g}\end{array}\right]$$
• 噪声传播得到协方差传播，注意这是预积分量的协方差。除非你起始就开始积分，才代表状态量。注意，刚开始时 ${\Sigma }_k=0$
  $${\Sigma }_k=F_{k-1}\cdot {\Sigma }_{k-1}\cdot F_{k-1}^T+G_{k-1}\cdot {\Sigma }_n\cdot G_{k-1}$$

4. 相机相关知识

4.1 BA（Bundle Adjustment）

• 简介：BA是针对 一段时间内 相机与被观测点构成的观测关系，进行相机Pose、被观测点LandMark的Pose最优化的问题

5. 推荐链接：

1. 知乎：从零开始做自动驾驶定位
2. CSDN：自制SLAM框架