二、基于优化的IMU与视觉信息融合

---

一、最小二乘问题的求解

1、最小二乘问题的定义

2、最小二乘问题的求解方法
  对于线性系统的最小二乘问题，可以直接对损失函数求导，通过找极点找到最优值；但是对于非线性系统而言，其导函数往往比较复杂，直接求导的方法并不适用。对于非线性系统的问题可以从初始值开始使用迭代的方法不断的降低损失函数的大小，这类方法称为迭代下降法。
  对于迭代下降法，主要是找到增量derta_x大小和方向。增量的大小尽可能小，这样才会满足泰勒展开。可以对损失函数进行泰勒展开，

迭代下降法常用的有：

• 最速下降法
• 牛顿法
• 阻尼法
• 高斯牛顿法
• LM法

最速下降法
  最速下降法下降的方向为梯度的负方向，因为梯度的方向是函数值增长最快的方向，其缺点是最优质附近震荡，收敛较慢。因此最速下降法适用于迭代的开始阶段。

牛顿法
  将损失函数在x附近进行泰勒展开。如果x+增量 是最优解，那么右式就是取得最小值，

此时右式对于增量的导数为零。可以解的增量的大小，再进行迭代。

阻尼法
  求最优增量时加一个惩罚项，使得增量不要太大。

高斯牛顿法
  对残差函数进行一阶泰勒近似，然后带入到损失函数中，再求极小值，得到正则方程。
上式对增量求导得到正则方程

LM法
  LM法在求解的过程中引入了阻尼因子μ，这是因为高斯牛顿法中的H是半正定的，如果出现H为奇异矩阵的情况，可能导致算法不收敛，为了克服这一不足，在LM法中引入了阻尼因子，使得在LM法中的H始终保持正定，使迭代朝着下降的方向进行。

阻尼因子初始值的选取为：

在迭代过程中，阻尼因子的更新策略通过比例因子来确定：

最终LM法的框架为：

3、最小二乘问题的求解框架

二、残差函数的构建

协方差的引入是为了消除不同传感器之间的量纲和对不同的误差赋予不同的权重。

1、视觉重投影误差。
一个特征点在归一化坐标系下的估计值与观测值之差


右面为归一化坐标系，在高博的十四讲里，u,v用来表示像素坐标系，与归一化坐标系差一个相机的内参数。

最右式的那个列向量，是归一化坐标系变换到相机坐标系ci，然后经过变换矩阵，一步步变到相机坐标系cj，然后估计值-观测值作为重投影误差，即（24）式

2、IMU测量值积分

为了解决重新积分导致运算量较大的问题，采用预积分的方法。

预积分量是一个相对量，仅跟IMU测量值有关，将一段时间内的IMU数据直接积分起来就得到了预积分量，定义：



预积分的方差
一个IMU数据作为测量值的噪声方差可以标定，一段时间多个IMU数据积分形成的预积分量的方差可以通过方差传递的方式标定。

那么imu噪声和预积分量之间的线性递推关系是什么呢？
假设已知相邻时刻的线性传递方程

协方差矩阵可以通过递推计算得到：

可以通过两种方法推导状态误差传递的线性递推关系：

• 基于一阶泰勒展开的误差递推方程
• 基于误差随时间变化的递推方程
  
  
  预积分误差递推公式的推导：
  
  

三、残差Jacobian的推导