VIO学习笔记（三）—— 基于优化的 IMU 与视觉信息融合

学习资料是深蓝学院的《从零开始手写VIO》课程，对课程做一些记录，方便自己以后查询，如有错误还请斧正。由于习惯性心算公式，所以为了加深理解，文章公式采用手写的形式。

VIO学习笔记（一）—— 概述
VIO学习笔记（二）—— IMU 传感器

基于 Bundle Adjustment 的 VIO 融合

视觉 SLAM 里的 Bundle Adjustment 问题

已知:
  状态量初始值:特征点的三维坐标,相机的位姿。
  系统测量值:特征点在不同图像上的图像坐标。

解决方式:
  构建误差函数,利用最小二乘得到状态量的最优估计:
$$\underset{q,p,f}{\mathrm{argmin}}\sum _{i=1}^m\sum _{j=1}^n||\pi (q_{w{c}_i},p_{w{c}_i},f_j)-z_{f_j}^{c_i}|{|}_{{\Sigma }_{ij}}$$

符号定义:
  $q$: 旋转四元数
  $p$: 平移向量
  $f$: 特征点3D坐标
  $c_i$: 第ｉ个相机系
  $\pi (\cdot )$: 投影函数
  $Z_{f_j}^{c_i}$: $c_i$对$f_j$的观测
  ${\sum }_{ij}$: $\sum$范数

            

g2o or ceres 中采用如下的求解方式

VIO 信息融合问题

由于imu的数据采集频率大于相机的采集频率，所以在两个相机坐标系之间，存在多个imu坐标系即body坐标系，由于相机坐标系应根body坐标系相对应，所以对两图像间的imu数据进行预积分，使其变为一个。

预积分的简单理解

为什么要预积分，slam系统中为了减小优化求解器的负担，采用了关键帧策略，IMU的速率显然要快于关键帧的插入，它们之间的关系可以用下图很好的表示。

紧耦合的方式就是把imu和图像的信息共同来估计状态量，所以如何协调两者之间的关系了，预积分干了这么一件事，通过重新参数化，把关键帧之间的IMU测量值积分成相对运动的约束，避免了因为初始条件变化造成的重复积分。

用IMU的slam、vio算法有很多，有滤波器的比如MSCKF，有基于图优化的比如VINS，OKVIS，ORB-SLAM等。就拿ORB-SLAM来说吧， 在bundle adjustment里，参与对象是keyframe，比如有2个keyframe： $K{F}_1,K{F}_2$, 他们的位姿分别为：$P_{1w},P_{2w}$,那么他们的相对位姿 :
$$P_{21}=P_{1w}\cdot P_{2w}^{-1}$$
我们可以认为$P_{21}$为估计项，是由SLAM的位姿直接算出来的。如果要构成一个优化问题，我们还需要知道误差项和测量项。没错，是IMU可以计算在这两个KF间的测量项 $P_{imu}$，预计分干的事情就是计算这个测量项。
$$r=P_{21}\cdot P_{imu}^{-1}$$
在紧耦合的优化slam中，IMU就是提供了两个关键帧的相对测量，从而构建误差函数对关键帧姿态的迭代优化。当然实际应用中不会是这么简单的形式，这里面要对各个变量分别求取误差，然后求雅克比矩阵。

最小二乘问题的求解

最小二乘基础概念

定义

找到一个 n 维的变量$x^{\ast }\in R^n$ ,使得损失函数 F (x) 取局部最小值:
$$F(x)=\frac{1}{2}\sum _{i=1}^m(f_i(x){)}^2$$
其中$f_i$是残差函数,比如测量值和预测值之间的差,且有 m ≥ n。局部最小值指对任意 $\Vert x-x^{\ast }\Vert <\delta$ 有 $F(x^{\ast })\le F(x)$

损失函数泰勒展开

假设损失函数 $F(x)$ 是可导并且平滑的,因此,二阶泰勒展开:
$$F(x+∆x)=F(x)+J∆x+\frac{1}{2}∆x^{\top }H∆x+O(\Vert ∆x{\Vert }^3)$$
其中 J 和 H 分别为损失函数 F 对变量 x 的一阶导和二阶导矩阵。

损失函数泰勒展开性质

忽略泰勒展开的高阶项,损失函数变成了二次函数,可以轻易得到如下性质:
  如果在点 $x_s$ 处有导数为 0 ,则称这个点为稳定点。
  在点 $x_s$ 处对应的 Hessian 为 H:
    如果是正定矩阵,即它的特征值都大于 0,则在 $x_s$ 处有 $F(x)$ 为局部最小值;
    如果是负定矩阵,即它的特征值都小于 0,则在 $x_s$ 处有 $F(x)$ 为局部最大值;
    如果是不定矩阵,即它的特征值大于 0 也有小于 0 的,则 $x_s$ 处为鞍点。

求解法

直接求解:线性最小二乘。
迭代下降法:适用于线性和非线性最小二乘。

迭代法初衷

找一个下降方向使损失函数随 x 的迭代逐渐减小,直到 x 收敛到 $x^{\ast }$ :
$$F(x_{k+1})<F(x_k)$$
分两步:
  第一,找下降方向单位向量 d,
  第二,确定下降步长 α.

假设 α 足够小,我们可以对损失函数 F (x) 进行一阶泰勒展开:
$$F(x+\alpha d)\approx F(x)+\alpha Jd$$
只需寻找下降方向,满足:
$$Jd<0$$
通过 line search 方法找到下降的步长:${\alpha }^{\ast }=argmi{n}_{\alpha >0}F(x+\alpha d)$

对损失函数泰勒展开

最速下降法(一阶梯度法): 适用于迭代的开始阶段

从下降方向的条件可知:$Jd=\Vert J\Vert cos\theta$,$\theta$ 表示下降方向和梯度方向的夹角。当 $\theta =\pi$ ,有
$$d=\frac{-J^T}{\Vert J\Vert }$$
即梯度的负方向为最速下降方向。缺点:最优值附近震荡,收敛慢。

牛顿法(二阶梯度法):适用于最优值附近

在局部最优点 $x^{\ast }$ 附近,如果 $x+∆x$ 是最优解,则损失函数对 ∆x的导数等于 0,对损失函数的二阶泰勒展式取一阶导有:
$$\frac{\partial }{\partial ∆x}(F(x)+J∆x+\frac{1}{2}∆x^{\top }H∆x)=J^{\top }+H∆x=0$$
得到:$∆x=-H^{-1}{J}^{\top }$ 。缺点:二阶导矩阵计算复杂。

Damp Method

将损失函数的二阶泰勒展开记作
$$F(x+∆x)\approx L(∆x)\equiv F(x)+J∆x+∆x^{\top }H∆x$$
求以下函数的最小化:
$$∆x\equiv arg\underset{∆x}{\min }(L(∆x)+\frac{1}{2}\mu ∆x^{\top }∆x)$$
其中,$\mu \ge 0$为阻尼因子, $\frac{1}{2}\mu ∆x^{\top }∆x=\frac{1}{2}\mu \Vert ∆x{\Vert }^2$是惩罚项(不让步长过大)。
对新的损失函数求一阶导,并令其等于 0 有:
$$\begin{gathered} L^{\prime }(∆x)+\mu ∆x=0 \\ \Rightarrow (H+\mu I)∆x=-J^{\top } \end{gathered}$$

对残差函数泰勒展开

符号说明

为了公式约简,可将残差组合成向量的形式。
$$\vec{f}(x)=\left[\begin{array}{c}{f}_1(x)\\ f_2(x)\\ \dots \\ f_1(x)\end{array}\right]$$
则有:　　$f^{\top }(x)f(x)={\sum }_{i=1}^m(f_i(x){)}^2$
同理,如果记 ${\vec{J}}_i(x)=\frac{\partial f_i(x)}{\partial x}$ 则有:
$$\frac{\partial \vec{f}(x)}{\partial x}=\vec{J}=\left[\begin{array}{c}{\vec{J}}_1(x)\\ {\vec{J}}_2(x)\\ \dots \\ {\vec{J}}_i(x)\end{array}\right]$$

基础

残差函数 $\vec{f}(x)$ 为非线性函数,对其一阶泰勒近似有:
$$f(x+∆x)\approx l(∆x)\equiv f(x)+J∆x$$
请特别注意,这里的 J 是残差函数 f 的雅克比矩阵。代入损失函数:
$$\begin{gathered} F(x+∆x)\approx L(∆x)\equiv \frac{1}{2}l(∆x{)}^{\top }l(∆x) \\ =\frac{1}{2}{f}^{\top }f+∆x^{\top }{J}^{\top }f+\frac{1}{2}∆x^{\top }{J}^{\top }J∆x \\ =F(x)+∆x^{\top }{J}^{\top }f+\frac{1}{2}∆x^{\top }{J}^{\top }J∆x \end{gathered}$$
这样损失函数就近似成了一个二次函数,并且如果雅克比是满秩的,则 $J^{\top }J$ 正定,损失函数有最小值。
另外, 易得:$F^{\prime }(x)=(J^{\top }f{)}^{\top }$ ,以及$F^{\prime \prime }(x)\approx J^{\top }J$.

Gauss-Newton Method

令损失函数就近似式的一阶导等于 0,得到:
$$J^{\top }J∆x_{gn}=-J^{\top }f$$
上式就是通常论文里看到的 $H∆x_{gn}=b$,称其为 normal equation.

Levenberg-Marquardt Method

Levenberg (1944) 和 Marquardt (1963) 先后对高斯牛顿法进行了改进,求解过程中引入了阻尼因子:
$$(J^{\top }J+\mu I)∆x_{lm}=-J^{\top }fwith\mu \ge 0$$

阻尼因子的作用

$\mu >0$ 保证 $(J^{\top }J+\mu I)$ 正定,迭代朝着下降方向进行。
$\mu$ 非常大,则 $∆x_{lm}=-\frac{1}{\mu }{J}^{\top }f=-\frac{1}{\mu }{F}^{\prime }(x{)}^{\top }$ , 接近最速下降法.
$\mu$ 比较小,则 $∆x_{lm}\approx ∆x_{gn}$ , 接近高斯牛顿法。

阻尼因子初始值的选取

阻尼因子 $\mu$ 大小是相对于 $J^{\top }J$ 的元素而言的。半正定的信息矩阵$J^{\top }J$特征值 {${\lambda }_j$} 和对应的特征向量为 {$v_j$}。对 $J^{\top }J$ 做特征值分解分解后有:$J^{\top }J=V\Lambda V^{\top }$ 可得(不知道咋算出来的，先记住接着向下走吧，有时间再补特征分解的知识！！！):
$$∆x_{lm}=-\sum _{j=1}^n\frac{v_j^T{F}^{{}^{\prime }T}}{{\lambda }_j+\mu }{v}_j$$
所以,一个简单的 ${\mu }_0$ 初始值的策略就是:
$${\mu }_0=\tau \cdot \max (J^{\top }J{)}_{ii}$$
通常,按需设定 $\tau \sim [10^{-8},1]$。

阻尼因子 μ 的更新策略

1. 定性分析,直观感受阻尼因子的更新:
     如果 $∆x\to F(x)\uparrow$ ,则 $\mu \uparrow \to ∆x\downarrow$, 增大阻尼减小步长,拒绝本次迭代。
     如果 $∆x\to F(x)\downarrow$ ,则 $\mu \downarrow \to ∆x\uparrow$, 减小阻尼增大步长。加快收敛,减少迭代次数。
2. 定量分析,阻尼因子更新策略通过比例因子来确定的:
   $$\rho =\frac{F(x)-F(x+∆x_{lm})}{L(0)-L(∆x_{lm})}$$
   其中:
   则有：
   $$\rho =\frac{F(x)-F(x+∆x_{lm})}{\frac{1}{2}∆x_{lm}^T(\mu ∆x_{lm}+b)}b=-J^{T}f$$

Marquardt 策略

首先比例因子分母始终大于 0,如果:
  $\rho <0$, 则 $F(x)\uparrow$ ,应该 $\mu \uparrow \to ∆x\downarrow$, 增大阻尼减小步长。
  如果 $\rho >0$ 且比较大,减小 $\mu$, 让 LM 接近 Gauss-Newton 使得系统更快收敛。
  反之,如果是比较小的正数,则增大阻尼 $\mu$,缩小迭代步长。
1963 年 Marquardt 提出了一个如下的阻尼策略:
$$\begin{gathered} if\rho <0.25 \\ \mu :=\mu \ast 2 \\ elseif\rho >0.75 \\ \mu :=\mu /3 \end{gathered}$$
Marquardt 好不好呢?如下图所示 :
          
会看到 $\mu$存在一定的跳动，这就导致步长存在一定的晃动，不稳定。

Nielsen 策略 (被 g2o, ceres 采用)

$$\begin{gathered} if\rho >0 \\ \mu :=\mu \ast max(\frac{1}{3},1-(2\rho -1{)}^3);\nu :=2 \\ else \\ \mu :=\mu \ast \nu ;\nu :=2\ast \nu \end{gathered}$$
        

鲁棒核函数的实现

引言:最小二乘中遇到 outlier 怎么处理?核函数如何在代码中实现?有多种方法,这里主要介绍 g2o 和 ceres 中使用的 Triggs Correction .

Triggs Correction

鲁棒核函数直接作用残差 $f_k(x)$ 上,最小二乘函数变成了如下形式:
$$\underset{x}{\min }\frac{1}{2}\sum _k\rho (\Vert f_k(x){\Vert }^2)$$
将误差的平方项记作 $s_k=\Vert f_k(x){\Vert }^2$ , 则鲁棒核误差函数进行二阶泰勒展开有:
$$\frac{1}{2}\rho (s)=\frac{1}{2}(const+{\rho }^{\prime }∆s+{\rho }^{\prime }\prime {∆}^{2}s)$$
上述函数中 $∆s_k$ 的计算稍微复杂一点:

$∆s_k$ 代入$\frac{1}{2}\rho (s)$有:

代三角号的计算步骤很蒙，没有算出来，找了一下论文，发现论文也是这样写的：
            
估计是哪个地方的知识点不到位把，再研究研究！！！

对上式求和后,对变量 ∆x 求导,令其等于 0 ,得到:

Example: Cauchy Cost Function

柯西鲁棒核函数的定义为:
$$\rho (s)=c^{2}log(1+\frac{s}{c^2})$$
其中 $c$ 为控制参数。对 $s$ 的一阶导和二阶导为:
$${\rho }^{\prime }(s)=\frac{1}{1+\frac{s}{c^2}},{\rho }^{\prime \prime }(s)=-\frac{1}{c^2}({\rho }^{\prime }(s){)}^2$$

核函数拓展

核函数控制参数的设定

95% efficiency rule (Huber, 1981):itprovides an asymptotic efficiency 95% that of linear regression for the normal distribution.
• 如果残差 $f_i$ 是正态分布,Huber
  $c=1.345$,
  $Cauchyc=2.3849$
• 如果残差非正态分布,需估计残差方差,然后对残差归一化。median absolute residual 方法
  $\sigma =1.482\cdot med(med(r)-r_i)$.

回顾最小二乘求解

1. 找到一个合适的关于状态量 x 的残差函数 $f_i(x)$,后续用 r, err等表示。
2. 计算残差函数对状态量 x 的雅克比 J。
3. 选定 cost function 以及其参数。
4. LM 算法求解。

VIO 残差函数的构建

视觉重投影误差

视觉重投影误差

定义:
  一个特征点在归一化相机坐标系下的估计值与观测值的差。
$$r_c=\left[\begin{array}{c}\frac{x}{z}-u\\ \frac{y}{z}-v\end{array}\right]$$
其中,待估计的状态量为特征点的三维空间坐标 $(x,y,z{)}^{\top }$ ,观测值$(u,v{)}^{\top }$ 为特征在相机归一化平面的坐标。

逆深度参数化

特征点在归一化相机坐标系与在相机坐标系下的坐标关系为:
$$\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\frac{1}{\lambda }\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]$$
其中 $\lambda =1/z$ 称为逆深度(更接近于高斯分布)。

VIO 中基于逆深度的重投影误差

        
特征点逆深度在第 i 帧中初始化得到,在第 j 帧又被观测到,预测其在第 j 中的坐标为:
$$\left[\begin{array}{c}{x}_{c_j}\\ y_{c_j}\\ z_{c_j}\\ 1\end{array}\right]=T_{bc}^{-1}{T}_{w{b}_j}^{-1}{T}_{w{b}_i}{T}_{bc}\left[\begin{array}{c}\frac{1}{\lambda }{u}_{c_i}\\ \frac{1}{\lambda }{v}_{c_i}\\ \frac{1}{\lambda }\\ 1\end{array}\right]$$
将i帧中观测到的数据变换到相机坐标系，将相机坐标系变换到body坐标系，将第i个body坐标系变换到世界坐标系，将世界坐标系变换到第j个body坐标系，将body坐标系变换到相机坐标系，得到第j帧的预测值。这期间相对于纯视觉多了相机坐标系变换到body坐标系，然后再由body坐标系变换回相机坐标系的过程。
视觉重投影误差为:
$$r_c=\left[\begin{array}{c}\frac{x_{c_i}}{z_{c_i}}-u_{c_i}\\ \frac{y_{c_i}}{z_{c_i}}-u_{c_i}\end{array}\right]$$

IMU 测量值积分

IMU 的真实值为 $\omega$, $a$, 测量值为 $\tilde{\omega }$, $\tilde{a}$,则有:
$$\begin{gathered} {\tilde{\omega }}^b={\omega }^b+b^g+n^g \\ \tilde{a}=q_{bw}(a^w+g^w)+b^a+n^a \end{gathered}$$
上标 $g$ 表示 gyro,$a$ 表示 acc,$w$ 表示在世界坐标系 world,$b$ 表示imu 机体坐标系 body。
PVQ 对时间的导数可写成:
$$\begin{gathered} {\dot{p}}_{w{b}_t}=v_t^w \\ {\dot{v}}_t^w=a_t^w \\ {\dot{q}}_{w{b}_t}=q_{w{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{w}^{b_t}\end{array}\right] \end{gathered}$$

测量值对世界坐标的积分

从第 i 时刻的 PVQ 对 IMU 的测量值进行积分得到第 j 时刻的 PVQ:
      
$$\begin{gathered} p_{w{b}_j}=p_{w{b}_i}+v_i^w∆t+{\iint }_{t\in [i,j]}(q_{w{b}_t}{a}^{bt}-g^w)\delta t^2 \\ {v}_j^w=v_i^w+{\int }_{t\in [i,j]}(q_{w{b}_t}{a}^{b_t}-g^w)\delta t \\ {q}_{w{b}_j}={\int }_{t\in [i,j]}{q}_{w{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{w}^{b_t}\end{array}\right] \end{gathered}$$
每次 q wb t 优化更新后,都需要重新进行积分,运算量较大。所以引出下文预积分解决方案。

IMU 预积分

一个很简单的公式转换,就可以将积分模型转为预积分模型:
$$q_{w{b}_t}=q_{w{b}_i}\otimes q_{b_i{b}_t}$$
那么,PVQ 积分公式中的积分项则变成相对于第 i 时刻的姿态,而不是相对于世界坐标系的姿态:
$$\begin{gathered} p_{w{b}_j}=p_{w{b}_i}+v_i^w∆t-\frac{1}{2}{g}^w∆t^2+q_{w{b}_i}{\iint }_{t\in [i,j]}(q_{b_i{b}_t}{a}^{bt})\delta t^2 \\ {v}_j^w=v_i^w-g^w∆t+q_{w{b}_i}{\int }_{t\in [i,j]}(q_{b_i{b}_t}{a}^{bt})\delta t \\ {q}_{w{b}_j}=q_{w{b}_i}{\int }_{t\in [i,j]}{q}_{b_i{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{w}^{b_t}\end{array}\right] \end{gathered}$$

预积分量

预积分量仅仅跟 IMU 测量值有关,它将一段时间内的 IMU 数据直接积分起来就得到了预积分量:
$$\begin{gathered} {\alpha }_{b_i{b}_j}={\iint }_{t\in [i,j]}(q_{b_i{b}_t}{a}^{bt})\delta t^2 \\ {\beta }_{b_i{b}_j}={\int }_{t\in [i,j]}(q_{b_i{b}_t}{a}^{bt})\delta t \\ {q}_{b_i{b}_j}={\int }_{t\in [i,j]}{q}_{b_i{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{w}^{b_t}\end{array}\right] \end{gathered}$$
重新整理下 PVQ 的积分公式,有:
$$\left[\begin{array}{c}{p}_{w{b}_j}\\ v_j^w\\ q_{w{b}_j}\\ b_j^a\\ b_j^g\end{array}\right]=\left[\begin{array}{c}{p}_{w{b}_i}+v_i^w∆t-\frac{1}{2}{g}^w∆t^2+q_{w{b}_i}{\alpha }_{b_i{b}_j}\\ v_i^w-g^w∆t+q_{w{b}_i}{\beta }_{b_i{b}_j}\\ q_{w{b}_i}{q}_{b_i{b}_j}\\ b_i^a\\ b_i^g\end{array}\right]$$

预积分误差

定义:
  一段时间内 IMU 构建的预积分量作为测量值,对两时刻之间的状态量进行约束,
$${\left[\begin{array}{c}{r}_p\\ r_v\\ r_q\\ r_{ba}\\ r_{bg}\end{array}\right]}_{15\times 1}=\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}\\ q_{b_{i}w}(v_j^w-v_i^w+g^w∆t)-{\beta }_{b_i{b}_j}\\ 2[q_{b_j{b}_i}\otimes (q_{b_{i}w}\otimes q_{w{b}_j}){]}_{xyz}\\ b_j^a-b_i^a\\ b_j^g-b_i^g\end{array}\right]$$
上面误差中位移,速度,偏置都是直接相减得到。第二项是关于四元数的旋转误差,其中 [·] xyz 表示只取四元数的虚部 (x, y, z) 组成的三维向量。

预积分的离散形式

这里使用 mid-point 方法,即两个相邻时刻 k 到 k+1 的位姿是用两个时刻的测量值 a, ω 的平均值来计算:
        

预积分量的方差

疑问:

  一个 IMU 数据作为测量值的噪声方差我们能够标定。现在,一段时间内多个 IMU 数据积分形成的预积分量的方差呢?

Covariance Propagation(协方差传播)

已知一个变量 $y=Ax,x\in N(0,{\Sigma }_x)$, 则有 ${\Sigma }_y=A{\Sigma }_x{A}^{\top }$
$$\begin{gathered} {\Sigma }_y=E((Ax)(Ax{)}^{\top }) \\ =E(Ax{x}^{\top }{A}^{\top }) \\ =A\Sigma x{A}^{\top } \end{gathered}$$
所以,要推导预积分量的协方差,我们需要知道imu 噪声和预积分量之间的线性递推关系。
假设已知了相邻时刻误差的线性传递方程:
$${\eta }_{ik}=F_{k-1}{\eta }_{ik-1}+G_{k-1}{n}_{k-1}$$
比如:状态量误差为 ${\eta }_{ik}=[\delta {\theta }_{ik},\delta v_{ik},\delta p_{ik}]$,测量噪声为$n_k=[n_k^g,n_k^a]$。
误差的传递由两部分组成:当前时刻的误差传递给下一时刻,当前时刻测量噪声传递给下一时刻。

一个有趣的例子

综艺节目中常有传递信息的节目,前一个人根据上一个人的信息 + 自己的理解(测量)传递给下一个人,导致这个信息越传越错。
协方差矩阵可以通过递推计算得到:
$${\Sigma }_{ik}=F_{k-1}{\Sigma }_{ik-1}{F}_{k-1}^{\top }+G_{k-1}{\Sigma }_n{G}_{k-1}^T$$
其中,${\Sigma }_n$ 是测量噪声的协方差矩阵,方差从 i 时刻开始进行递推,${\Sigma }_{ii}=0$。

状态误差线性递推公式的推导

简介

通常对于状态量之间的递推关系是非线性的方程如$x_k=f(x_{k-1},u_{k-1})$,其中状态量为 $x$,$u$ 为系统的输入量。
我们可以用两种方法来推导状态误差传递的线性递推关系:
  一种是基于一阶泰勒展开的误差递推方程。
  一种是基于误差随时间变化的递推方程。

基于泰勒展开的误差传递(应用于 EKF 的协方差预测)

令状态量为 $x=\hat{x}+\delta x$,其中,真值为 $\hat{x}$,误差为 $\delta x$。另外,输入量$u$ 的噪声为$n$。
非线性系统 $x_k=f(x_{k-1},u_{k-1})$ 的状态误差的线性递推关系如下:
$$\delta x_k=F\delta x_{k-1}+G{n}_{k-1}$$
其中,F 是状态量 $x_k$ 对状态量 $x_{k-1}$ 的雅克比矩阵,G 是状态量 $x_k$对输入量 $u_{k-1}$ 的雅克比矩阵。
证明:对非线性状态方程进行一阶泰勒展开有:
$$\begin{gathered} x_k=f(x_{k-1},u_{k-1}) \\ {\hat{x}}_k+\delta x_k=f({\hat{x}}_{k-1}+\delta x_{k-1},{\hat{u}}_{k-1}+n_{k-1}) \\ {\hat{x}}_k+\delta x_k=f({\hat{x}}_{k-1},{\hat{u}}_{k-1})+F\delta x_{k-1}+G{n}_{k-1} \end{gathered}$$

基于误差随时间变化的递推方程

如果我们能够推导状态误差随时间变化的导数关系,比如:
$$\delta x^{{}^{\prime }}=A\delta x+Bn$$
则误差状态的传递方程为:
$$\begin{gathered} \delta x_k=\delta x_{k-1}+\delta x_{k-1}^{{}^{\prime }}∆t \\ \to \delta x_k=(I+A∆t)\delta x_{k-1}+B∆t{n}_{k-1} \end{gathered}$$
这两种推导方式的可以看出有:
$$F=I+A∆t,G=B∆t$$

第一种方法不是很好么,为什么会想着去弄误差随时间的变化呢?

这是因为 VIO 系统中已经知道了状态的导数和状态之间的转移矩阵。如:我们已经知道速度和状态量之间的关系:
$$\dot{v}=R{a}^b+g$$
那我们就可以推导速度的误差和状态误差之间的关系,再每一项上都加上各自的误差就有:
$$\begin{gathered} \dot{v}+\delta \dot{v}=R(I+[\delta \theta {]}_{\times })(a^b+\delta a^b)+(g+\delta g) \\ \delta \dot{v}=R\delta a^b+R[\delta \theta {]}_{\times }(a^b+\delta a^b)+\delta g \\ \delta \dot{v}=R\delta a^b-R[a^b{]}_{\times }\delta \theta +\delta g \end{gathered}$$
由此就能依次类推,轻易写出整个 A 和 B 其他方程了。

预积分的误差递推公式推导

首先回顾预积分的误差递推公式,将测量噪声也考虑进模型:
     
确定误差传递的状态量,噪声量,然后开始构建传递方程。
用前面一阶泰勒展开的推导方式$\delta x_k=F\delta x_{k-1}+G{n}_{k-1}$,我们希望能推导出如下的形式:
            
F, G 为两个时刻间的协方差传递矩阵。
这里我们直接给出 F, G 的最终形式,后面会对部分项进行详细推导:
    
      
其中的系数为:

下面对Ｆ的第三行进行推导，其他行列的推导类似：

残差 Jacobian 的推导

视觉重投影残差的 Jacobian

视觉残差为:

对于第 i 帧中的特征点,它投影到第 j 帧相机坐标系下的值为:

拆成三维坐标形式为:

再推导各类 Jacobian 之前,为了简化公式,先定义如下变量:
                  
Jacobian 为视觉误差对两个时刻的状态量,外参,以及逆深度求导:
          
根据链式法则,Jacobian 的计算可以分两步走:
第一步误差对 $f_{c_j}$ 求导:
                  
第二步 f c j 对各状态量求导:

1. 对 i 时刻的状态量求导
   a. 对 i 时刻位移求导,可直接写出如下:
   
   b. 对 i 时刻角度增量求导
   
   上面公式和 i 时刻角度相关的量并不多,下面为了简化,直接丢弃了不相关的部分
   
   Jacobian 为
   
2. 对 j 时刻的状态量求导
   a. 对位移求导:
                 
   b. 对角度增量求导,同上面的操作,也简化一下公式
   
   Jacobian 为
           
3. 对 imu 和相机之间的外参求导
   a. 对位移求导
               
   b. 对角度增量求导,由于 f c j 都和 R bc 有关,并且比较复杂,所以这次分两部分求导
   
   第一部分 Jacobian 为
       
   分子可写成:
       
   那么,第一部分的 Jacobian 为:
       
   第二部分的 Jacobian 为:
    
   两个 Jacobian 相加就是视觉误差对外参中的角度增量的最终结果。
4. 视觉误差对特征逆深度的求导
   

IMU 误差相对于优化变量的 Jacobian

在求解非线性方程时,需要知道误差 e B 对两个关键帧 i, j 的状态量p, q, v, b a , b g 的 Jacobian。

对 i, j 时刻的状态量 p, q, v 求导还是比较直观的,直接对误差公式进行计算就行。但是对 i 时刻的 b ai , b gi 求导就显得十分复杂,下面我们详细讨论。
因为 i 时刻的 bias 相关的预积分计算是通过迭代一步一步累计递推的,可以算但是太复杂。所以对于预积分量直接在 i 时刻的 bias 附近用一阶泰勒展开来近似,而不用真的去迭代计算。
              
其中
    
表示预积分量对 i 时刻的 bias 求导。
这些雅克比根据前面讨论的协方差传递公式,能一步步递推得到:
                      
下面我们来讨论 IMU 误差相对于两帧的 PVQ 的 Jacobian:
由于 r p 和 r v 的误差形式很相近,对各状态量求导的 Jacobian 形式也很相似,所以这里只对 r v 的推导进行详细介绍。

1. 对 i 时刻位移 Jacobian
                     
2. 对 i 时刻旋转 Jacobian
       
   上式可写为:
         
3. 对 i 时刻速度 Jacobian:
                     
4. 对 i 时刻的加速度 bias 的 Jacobian,注意 bias 量只和预积分 β 有关:
                 

对ｒｐ的推导过程：
5. 对 i 时刻姿态求导
        
上式可化简为:
      
其中 [·] L 和 [·] R 为四元数转为左/右旋转矩阵的算子。
6. 角度误差对 j 时刻姿态求导
      
7. 角度误差对 i 时刻陀螺仪偏置 b gi
      
这公式……我傻了！！！

相关资料：

1. Kaj Madsen, Hans Bruun Nielsen, and Ole Tingleff. “Methods for non-linear . least squares problems”. In: (1999).
2. Christopher Zach. “Robust bundle adjustment revisited”. In: European Conference on Computer Vision. Springer. 2014, pp. 772–787.
3. Bill Triggs et al. “Bundle adjustment—a modern synthesis”. In: International workshop on vision algorithms. Springer.1999, pp. 298–372.
4. Kirk MacTavish and Timothy D Barfoot. “At all costs: A comparison of robust . cost functions for camera correspondence outliers”. In: 2015 12th Conference on Computer and Robot Vision. IEEE. 2015, . pp.62–69.