MSCKF 2.0 理论推导以及能观性分析

转载请注明作者和出处（https://blog.csdn.net/waittingforyou12）

MSCKF2.0

现在对论文【1】的公式进行梳理与推导，也是做一个记录。推导分为三部分，第一部分进行IMU运动模型的说明，第二部分对IMU模型的误差状态转移方程进行推导，第三部分对MSCKF系统进行能观性分析。

1. IMU运动模型

6轴imu由加速度计和陀螺仪组成，分别测量在IMU惯性坐标系下的角速度和加速度。它们对应的观测值分别收到bias和噪声的影响。

其中 $\omega_{m},\alpha_{m}$ 表示测量值， $^{I}\omega$ 表示在IMU惯性坐标系下的真值， $^{I}\alpha$ 表示在世界坐标系下的加速度真值。

对角速度进行积分可以获得相邻两个时刻之间的旋转变化，对加速度数值分别进行一次和二次积分分别可以得到在速度和位移。

在MSCKF中，作者采用四元数表示旋转关系，用 $^{I_{l+1}}_{I_{l}}\bar{q}$ 表示时刻 $l$ 到 $l+1$ 的相对旋转。为了获得这个相对旋转，采用四元素的微分方程，将估计的角速度

带入微分方程

就可以获得相邻两个时刻的位姿变换。基于此变换可以传递到对IMU orientation在全局坐标下的估计值：

同样的，根据IMU模型可以获得加速度的关系式

对加速度值进行一次和二次积分可以分别获得在全局坐标下相邻两个时刻的速度关系和位置关系：

其中：

2. IMU误差状态转移方程

基于第一部分的IMU运动和积分模型，现在对误差状态转移方程进行推导。

A: 旋转误差状态转移方程

首先在MSCKF1.0中，orientation的误差是定义在局部坐标系上，在MSCKF2.0中，作者提出改进将orientation的误差的定义在全局坐标系，在后续的推导可以看到，这个改进结合FEJ（First Estimate Jacobian）可以使能观性矩阵的秩是4，符合真实系统的性能。定义全局坐标下导致的一个直观改变是旋转扰动由左扰动变为右扰动。

顺便提一句，这里面对扰动做减法是采用JPL的四元数表达形式，而不是采用Hamilton的四元数表达形式，对局部扰动，全局扰动，以及扰动用加法还是减法不太理解的推荐阅读文献【2】。

根据上述的表达方式，我们可以建立相邻两个时刻的旋转表达式

上式中的每一项都可以写成估计 + 扰动的方式：

$^{I_{l}}_{G}R \simeq ^{I_{l}}_{G} \hat{R}(\mathbb{I}_{3} - \left \lfloor ^{G}\tilde{\theta}_{l\times} \right \rfloor)$

$^{I_{l+1}}_{G}R \simeq ^{I_{l+1}}_{G} \hat{R}(\mathbb{I}_{3} - \left \lfloor ^{G}\tilde{\theta}_{l+1\times} \right \rfloor)$

$^{I_{l+1}}_{I_{l}}R \simeq ^{I_{l+1}}_{I_{l}} \hat{R}(\mathbb{I}_{3} - \left \lfloor \tilde{\theta}_{\Delta t\times} \right \rfloor)$

将以上三个方程分别代入，两边展开化简并且忽略高次项，可以有如下的推到过程：

$\begin{align*} ^{I_{l+1}}_{G} \hat{R}(\mathbb{I}_{3} - \left \lfloor ^{G}\tilde{\theta}_{l+1\times} \right \rfloor) &= ^{I_{l+1}}_{I_{l}} \hat{R}(\mathbb{I}_{3} - \left \lfloor \tilde{\theta}_{\Delta t\times} \right \rfloor) ^{I_{l}}_{G} \hat{R}(\mathbb{I}_{3} - \left \lfloor ^{G}\tilde{\theta}_{l\times} \right \rfloor) \\ ^{I_{l+1}}_{G} \hat{R} - ^{I_{l+1}}_{G} \hat{R}\left \lfloor ^{G}\tilde{\theta}_{l+1\times} \right \rfloor) &= ^{I_{l+1}}_{G} \hat{R} - ^{I_{l+1}}_{G} \hat{R}\left \lfloor ^{G}\tilde{\theta}_{l\times} \right \rfloor - ^{I_{l+1}}_{I_{l}} \hat{R}* \left \lfloor \tilde{\theta}_{\Delta t\times} \right \rfloor *^{I_{l}}_{G} \hat{R} -O(n^2) \\ - ^{I_{l+1}}_{G} \hat{R}\left \lfloor ^{G}\tilde{\theta}_{l+1\times} \right \rfloor) &=- ^{I_{l+1}}_{G} \hat{R}\left \lfloor ^{G}\tilde{\theta}_{l\times} \right \rfloor -^{I_{l+1}}_{G} \hat{R} \left \lfloor ^{I_{l}}_{G} \hat{R}^{T} \tilde{\theta}_{\Delta t\times} \right \rfloor \\ \end{align*}$

其中，行2中，忽略了高次项，即误差项的平方，行2到行3利用了叉乘的性质：

$[t]_{\times }M = M^{-T}[M^{-1}t]_{\times }$

其中M为正交矩阵。

自然地，根据推到，可以得到旋转部分误差状态转移方程，如论文中公式34所示：

$^{G}\tilde{\theta}_{l+1} &=^{G}\tilde{\theta}_{l} + ^{I_{l}}_{G} \hat{R}^{T} \tilde{\theta}_{\Delta t}$

B: 速度误差状态转移方程

在IMU运动模型中，建立了前后两个时刻的在世界坐标系下速度的关系，重新梳理如下：

$^{G}\hat{V}_{l+1} = ^{G}\hat{V}_{l} + ^{G}_{I_{l}}\hat{R}\hat{S}_{l}+g\Delta t$

$\hat{S}_{l} =\int_{t_{l}}^{t_{l+1}} ^{I_{l}}_I{\tau }\hat{R}(a_{m}(\tau) - \hat{b}_{a}(\tau))d\tau$

在上式中，我们将每一个变量写成真值加误差的形式，于是有：

$\begin{align*} ^{G}V_{l+1} + ^{G}\tilde{V}_{l+1} &=^{G}V_{l} + ^{G}\tilde{V}_{l} + ^{G}_{I_{l}}\hat{R}(S_{l} + \tilde{S}_{l})+g\Delta t \\ ^{G}V_{l+1} + ^{G}\tilde{V}_{l+1} &= ^{G}V_{l} + ^{G}\tilde{V}_{l} + (^{G}_{I_{l}}R + \left \lfloor ^{G}_{I_{l}}\tilde{\theta}_{\times} \right \rfloor ^{G}_{I_{l}}\hat{R}) (S_{l} + \tilde{S}_{l})+g\Delta t \\ ^{G}V_{l+1} + ^{G}\tilde{V}_{l+1} &= ^{G}V_{l} + ^{G}\tilde{V}_{l} + ^{G}_{I_{l}}RS_{l} + ^{G}_{I_{l}}R\tilde{S}_{l} + \left \lfloor ^{G}_{I_{l}}\tilde{\theta}_{\times} \right \rfloor^{G}_{I_{l}}\hat{R} S_{l} + ^{G}_{I_{l}}\hat{R}\left \lfloor ^{G}_{I_{l}}\tilde{\theta}_{\times} \right \rfloor \tilde{S}_{l}+g\Delta t \\ ^{G}\tilde{V}_{l+1} &= ^{G}\tilde{V}_{l}+^{G}_{I_{l}}R\tilde{S}_{l} + \left \lfloor ^{G}_{I_{l}}\tilde{\theta}_{\times} \right \rfloor^{G}_{I_{l}}\hat{R} S_{l} \\ \end{align*}$

$\begin{align*} ^{G}\tilde{V}_{l+1} &= ^{G}\tilde{V}_{l} + ^{G}_{I_{l}}\hat{R}\tilde{S}_{l} + \left \lfloor ^{G}_{I_{l}}\tilde{\theta}_{\times} \right \rfloor^{G}_{I_{l}}\hat{R}\tilde{S}_{l} \\ ^{G}\tilde{V}_{l+1} &=^{G}\tilde{V}_{l} + ^{G}_{I_{l}}\hat{R}\tilde{S}_{l} -\left \lfloor ^{G}_{I_{l}}\hat{R}\tilde{S}_{l\times} \right \rfloor \quad ^{G}_{I_{l}}\tilde{\theta} \end{align*}$

其中行1到行2运用论文中的公式32，其余就是展开公式，除去误差高次项的操作。

C: 位置误差状态转移方程

因为本质上速度和位置的状态转移方程是相同的，因此仿照对速度误差的推到，可以得到位置误差状态转移方程，就不再进行具体的公式推导，直接给出论文的结论：

$^G\tilde{p}_{l+1} = -\left \lfloor \hat{R}_{l}^T\hat{y}_{l}_\times\right \rfloor^G\tilde{\theta_{l}} + ^G\tilde{V}_{l}\Delta t + ^G\tilde{p}_{l} + \hat{R}_{l}^T\tilde{y_{l}}$

D: 误差状态转移方程

综合角度，速度，位置的误差状态转移方程，化简为矩阵形式可得：

其中 $\Phi _{I_l}(\hat{X}_{I_{l+1}},\hat{X}_{I_{l}})$ 表示IMU误差状态从上一时刻 $l$ 转移到当前时刻 $l+1$ 的状态矩阵，可以进一步的简写如下：

可以从 $\Phi _{I_l}(\hat{X}_{I_{l+1}},\hat{X}_{I_{l}})$ 状态转移矩阵得出三个结论：

矩阵对角线上都是单位矩阵块，这表明当前时刻的误差量包含了前一时刻转移过来的误差。
前一时刻的速度误差通过乘以相邻两个时刻的时间 $\Delta t$ ，影响着下一时刻的位姿误差。
在 $I_{l}$ 时刻产生的旋转矩阵误差分别通过 $\hat{R}_{l}^T\hat{y}_{l}_\times$ ， $\hat{R}_{l}^T\hat{s}_{l}_\times$ 造成了速度项和位置项的累计误差。

3. MSCKF系统能观性分析

首先需要说明的是

1. 为了简化证明过程，论文中对能观性分析的推导公式没有包含陀螺仪和加速度计bias项，在[2]工作中已经证明了bias是可观的，作者在论文附录中也提供了包含bias项的证明。

2. 作者证明了，在给定线性系统（或者近似线性系统）下，EKF和MSCKF的观测方程是等价的，因此对MSCKF的能观性分析可以用EKF的能观性分析的方式。

考虑[ $k, k+m$ ]时间段内的状态变量为IMU状态和地图点3D位置，如下所示：

对于观测方程，我们知道如果特征点是在时刻 $\alpha_i + 1$ 处理，那么就意味着MSCKF中对于IMU状态的雅可比都是基于到时刻 $\alpha_i$ 时所获得的状态估计线性展开的，对地图点的雅可比都是基于三角化获得的估计位置处（ $^G\hat{p}_{f_{i}}$ ）线性展开的。特别说明的是，论文用

$\hat{\alpha}_{i|j}$ 表示对状态 $\alpha$ 在 $i$ 时刻的估计，这个估计是根据 $j$ 时刻的观测更新而得到的。根据上述说明，MSCKF中观测方程的雅可比可以描述为：

其中 $H_{I_{il}}(\hat{\pi}_{l|\alpha_{i}}, ^G\hat{p}_{f_{i}})$ 表示观测对于 $l$ 时刻的IMU状态的雅可比， $H_{f_{il}}(\hat{\pi}_{l|\alpha_{i}}, ^G\hat{p}_{f_{i}})$ 表示观测对于 $l$ 时刻的地图点的雅可比。

接下来具体推到观测方程中对于IMU状态和地图点的雅可比矩阵，首先有：

$\begin{bmatrix} ^{C_{l}}\hat{x}_{f_{i}}\\ ^{C_{l}}\hat{y}_{f_{i}}\\ ^{C_{l}}\hat{z}_{f_{i}} \end{bmatrix}=^{C_{l}}\hat{p}_{f_{i}} = ^C_{I}R\hat{R}_{l|\alpha_{i}}(^G\hat{p}_{f_{i}}-^G\hat{p}_{l|\alpha_{i}}) + ^{C}p_{I}$

即首先将地图点通过IMU位姿转换到IMU坐标系，再通过IMU与相机的外参转换到相机坐标系。接着转化到归一化平面获得观测：

$\begin{bmatrix} u_n \\ v_n \end{bmatrix} = \begin{bmatrix} \frac{^{C_{l}}\hat{x}_{f_{i}}}{^{C_{l}}\hat{z}_{f_{i}}}\\ \frac{^{C_{l}}\hat{y}_{f_{i}}}{^{C_{l}}\hat{z}_{f_{i}}} \end{bmatrix}$

链式法则获取观测对位姿和地图点的雅可比分别是：

其中：

能观性矩阵：

在控制系统中，定义能观性矩阵为：

能观性矩阵的零空间描述的是，给定一段时间内的观测值为已知信息，无法成功进行参数估计的状态空间。因此零空间的维度确定是多少，那么状态空间的不可观测的维度就是多少。

A: 理想状态下的能观性矩阵

理想状态下的能观性矩阵是通过在真实状态值处展开雅克比的获取的，因此根据能观性矩阵的定义给出化简后的形式：

其中 $\hat{\O}_{il}$ 表示在时刻 $l$ 对应特征点 $i$ 的能惯性矩阵，很自然的，如果在某一帧优化的特征地图点有 $n$ 个，以重投影误差为例，那么对应的观测矩阵的维度应该是 $2n * (9+3n)$ ,其中9只是简化说明IMU的状态变量为位置，和速度和姿态。现在重点推导一下 $\hat{\O}_{il}$ 是如何计算出来的。

从时刻 $k$ 时刻 $l$ 的能惯性矩阵定义为 $H_{l}\Phi_{l-1}\Phi_{l-2}\cdots\Phi_{k}$ ,分别将观测矩阵和IMU误差状态转移矩阵代入，有：

$\begin{align*} H_{l}\Phi_{l-1}\Phi_{l-2}\cdots\Phi_{k} &= \begin{bmatrix} \left \lfloor ^{G}\hat{p}_{fi} - ^{G}\hat{p}_{l} \right \rfloor_\times & -\mathbb{I}_{3} & 0_{3} \end{bmatrix} \begin{bmatrix} \mathbb{I}_{3} & 0_{3} & 0_{3}\\ -\left \lfloor ^G\hat{p}_{l}-^{G}\hat{p}_{l-1}-^G\hat{v}_{l-1}\Delta t_{1}-\frac{1}{2}g\Delta t_{1}^2 \right \rfloor_\times& \mathbb{I}_{3}& \Delta t \mathbb{I}_{3}\\ -\left \lfloor ^{G}\hat{v}_{l}- ^{G}\hat{v}_{l-1} - g\Delta t_{1}\right \rfloor& 0_{3}& \mathbb{I}_{3} \end{bmatrix} \Phi_{l-2}\cdots\Phi_{k} \end{align*}$

$= \begin{bmatrix} \left \lfloor ^{G}\hat{p}_{fi} - ^{G}\hat{p}_{l-1} - ^{G}\hat{v}_{l-1}\Delta t_{1} - \frac{1}{2}g\Delta t_{1}^2\right \rfloor_\times & -\mathbb{I}_{3} & -\Delta t_{1}\mathbb{I}_{3} \end{bmatrix} \begin{bmatrix} \mathbb{I}_{3} & 0_{3} & 0_{3}\\ -\left \lfloor ^G\hat{p}_{l-1}-^{G}\hat{p}_{l-2}-^G\hat{v}_{l-2}\Delta t_{2}-\frac{1}{2}g\Delta t_{2}^2 \right \rfloor_\times & \mathbb{I}_{3} & \Delta t_{2} \mathbb{I}_{3}\\ -\left \lfloor ^{G}\hat{v}_{l-1}- ^{G}\hat{v}_{l-2} - g\Delta t_{2}\right \rfloor_{\times}& 0_{3}& \mathbb{I}_{3} \end{bmatrix} \Phi_{l-3}\cdots\Phi_{k}$

$= \begin{bmatrix} \left \lfloor ^{G}\hat{p}_{fi} - ^{G}\hat{p}_{l-2} - ^{G}\hat{v}_{l-2}(\Delta t_1 + \Delta t_2 )- \frac{1}{2}g(\Delta t_{1} + \Delta t_2)^2 \right \rfloor_\times & -\mathbb{I}_{3} & -(\Delta t_1 + \Delta t_{2})\mathbb{I}_{3} \end{bmatrix} \Phi_{l-3}\cdots\Phi_{k}$

$= \begin{bmatrix} \left \lfloor ^{G}\hat{p}_{fi} - ^{G}\hat{p}_{k} - ^{G}\hat{v}_{k}\Delta t_{l} - \frac{1}{2}g\Delta t_{l} ^2 \right \rfloor_\times & -\mathbb{I}_{3} & -\Delta t_{l}\mathbb{I}_{3} \end{bmatrix}$

注：上述推导中的状态其实对应的应该是真实值，但是在编辑过程中由于疏忽采用的是估计值的表示方式，不影响推导过程。

推导过程比较简单，唯一需要的是耐心的进行矩阵相乘并进行化简，在推导过程中可以很明显的发现，正是因为观测矩阵的雅克比展开以及状态转移矩阵都是在真实值处进行的，因此在后续的迭代推导中，会很明显的一一消除掉特征点在各个时刻的位置以及速度项 $v\Delta t$ 。

要进一步的确定MSCKF系统的不可观维度，那么需要判断能观性矩阵零空间的秩，作者通过拼凑的方式直接给出了能观性矩阵的零空间，如下所示：

$N= \begin{bmatrix} 0_{3}& g\\ \mathbb{I}_{3}& -\left \lfloor ^{G}p_{k}_\times \right \rfloor g \\ 0_{3}& -\left \lfloor ^{G}v_{k}_\times \right \rfloor g\\ \mathbb{I}_{3}& -\left \lfloor ^{G}p_{f_{1}}_\times \right \rfloor g\\ \mathbb{I}_{3}& -\left \lfloor ^{G}p_{f_{2}}_\times \right \rfloor g \\ \vdots & \vdots\\ \mathbb{I}_{3}& -\left \lfloor ^{G}p_{f_{N}}_\times \right \rfloor g \end{bmatrix}$

能观性矩阵与N相乘化简便可以得出 $\hat{\O}_{il} \cdot N = 0_{2\times4}$ 。

B: 理想状态下能观性矩阵的零空间维度

证明过程来自于文献[]中附录C的推导

其中第二列矩阵块乘以 $-\left \lfloor ^{G}p_{k}_\times \right \rfloor$ 并且加到第一列矩阵块有

第三列矩阵块乘以 $-\left \lfloor ^{G}v_{k}_\times \right \rfloor$ 并且加到第一列矩阵块有

对应特征点 $i$ 的列矩阵块乘以 $-\left \lfloor ^{G}p_{f_i}_\times \right \rfloor$ 并且加到第一列矩阵块有

将第一列矩阵块乘以2有

最后将所有特征点的列都与第二列矩阵块相加有

从上述化简出的能观性矩阵可以看出，矩阵的第3列到第6列都是0矩阵，并且注意到矩阵的前三列组成的矩阵块是一个斜对称矩阵，斜对称矩阵的祑为2，因此可以直接得出结论，上述能观性矩阵降秩至少为4。作者在论文中进一步的通过酉矩阵来辅助进行推导。首先定义单位正交矩阵：

这里的单位向量 $g_{p1} ,p_{p2}$ 组成的单位平面是垂直于重力加速度的矢量的，由于正交矩阵是满秩的，因此可以将它乘以能观性矩阵第一列，得到：

现在只需要证明能观性矩阵降秩确定为4，那么忽略到0矩阵的列，有：

后续证明过程略去，参考文献【3】附录C。

C: 零空间的物理意义解释

由于理想MSCKF能观性矩阵的零空间维度是4，所以可以写成如下形式：

分别对应零空间的四列，为了理解零空间基底的物理意义，只需要检验每一列的改变会对应改变相应的哪些状态空间量。首先假设初始状态为 $x$ , 然后通过零空间的前三列基底改变状态空间为 $x^{'} = x + c_1n_1 + c_2n_2+c_3n_3$ ，可以看到 $x^{'}$ 对应的姿态和速度都不会受到影响，而位置项将会发生改变，因此零空间前三列向量组成的基底改变会相应的改变状态空间中的位置项。

接着，如果我们以重力加速度方向为旋转轴将状态空间旋转一个非常小的角度，有:

很明显的，如果要证明 $n_4$ 基底会对绕重力加速度的旋转角度产生影响，就只需要证明状态 $x^{'}$ 与 $x$ 的差值与 $n_4$ 线性相关。

首先用 $^{G^{'}}_{G}R$ 表示绕重力加速度方向的旋转矩阵，这个旋转矩阵可以用下式近似表达：

其中 $^{G}\bar{g}$ 表示重力方向的单位矢量，根据上述近似表达可以得到旋转前与旋转后的状态差:

接着假设 $\delta \theta$ 代表状态向量 $x^{'}$ 与 $x$ 的姿态差，那么有：

从上式的最后一项可以得出结论：

结合上述两个证明推导，可以得出结论，当对状态空间绕重力加速度方向进行旋转时，状态空间的改变量线性相关于 $\frac{c}{\left \| G_g \right \|_2}n_4$ 。

至此，可以最后得出结论，零空间对应的四个基底分别对应于状态空间中的位置X,Y,Z以及绕重力加速度方向的旋转。

D: MSCKF系统实际能观性矩阵及零空间维度

如果实际的对A部分理想能观性矩阵进行过公式推导就会发现，在迭代推导过程中，由于使用的是状态的真值，因此会不断的一一消除掉特征点在各个时刻的位置以及速度项 $v\Delta t$ 。然而在真值的MSCKF系统中，我们知道会首先利用IMU的状态转移方程对IMU状态进行一个初步估计，在使用特征点的观测信息对状态进行更新，因此更新前后的状态是不同的，它们之间有着一个差值。因此按照A中的推导对实际系统的能观性矩阵进行推导有：

其中：

可以看到， $\Delta \Gamma _{il}$ 项是多出的扰动项，其形式比较复杂，取决于每次滤波器对状态的修正过程，由于这个修正过程是随机的，因此

$\Gamma _{il}$ 项对应的列矩阵块的祑就是不再是2，而是3。采用B中相同的分析方式也可以同样证明能观性矩阵降祑为3。这三个维度分别对应状态空间的X,Y,Z位置量。也就是说在真实的MSCKF系统中，估计器错误的从本身不可观的维度获得了更多的信息，造成的结果是对yaw角估计的不确定度降低，进一步的影响对其他状态空间量的估计，造成了系统的不一致性。

E: 提高MSCKF系统一致性的改进方法

由D中给出结论，MSCKF系统的不一致性主要来源于在观测方程中在不同线性点获取雅克比，这导致预测与更新的中的状态不同，导致出现扰动项。明显的方式是直接去除这个扰动项，如果在观测方程中，利用预测的状态，在其附近推导雅克比，就可以让消除这个状态不一致性。如下所示：

一句话总结就是永远利用状态变量的第一次估计值。即用 $\hat{x}_{l|l-1}$ 替代 $\hat{x}_{l|l}$ 。前者是第一次利用IMU状态转移方程获取的预测值，后者是基于此预测和观测进行滤波更新的值。

个人观点：虽然这种方式能够保证能观性矩阵符合真实系统的特性，然而由于每次在观测方程上都是基于第一次预测值附近展开雅克比进行线性近似，如果本身预测值偏离真实值特别多，那么这种近似展开的方式是相对不精确的，也间接的影响了优化的效果。

至此，该论文中主要思想以及公式推导都已经记录完成，建议阅读MSCKF源码对该部分进行更好的理解。

参考文献

[1] High-Precision, Consistent EKF-based Visual-Inertial Odometry

[2] Quaternion Kinematics for the error-state Kalman Filter

[3] Consistency of EKF-Based Visual-Inertial Odometry