MSCKF（三）——更新部分

Reference

1. A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation. MSCKF1.0的论文；
2. Quaternion Kinematics for Error-State KF. 关于四元数以及ESKF的论文；
3. Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight. S-MSCKF对应的论文；
4. https://github.com/KumarRobotics/msckf_vio S-MSCKF的工程；
5. https://zhuanlan.zhihu.com/p/76341809 知乎上大佬对MSCKF的总结，本文没有过多的程序方面的讲解，都是从理论上推导的，也是个人的一些解惑过程；
6. https://blog.csdn.net/wubaobao1993/article/details/109299097 笔者关于预测部分的总结；

 

---

参数的表示

首先重新回顾一下MSCKF的要估计的参数：
$${\tilde{\mathrm{X}}}_{IMU}={\left[\begin{array}{ccccccc}{}_G^I\delta {\theta }^T& {\tilde{b}}_g^T& {}^G{\tilde{v}}_I^T& {\tilde{b}}_a^T& {}^G{\tilde{p}}_I^T& {}_I^C\delta {\theta }^T& {}^I{\tilde{p}}_C^T\end{array}\right]}^T\text{\hspace{1em}(1)}$$
整个过程中的参数向量为：
$${\tilde{\mathrm{X}}}_{\mathrm{k}}={\left[\begin{array}{cccccc}{\tilde{\mathrm{X}}}_{\mathrm{I}\mathrm{M}\mathrm{U}}& \delta {\theta }_{C_1}& {}^G{\tilde{p}}_{C_1}^T& ...& \delta {\theta }_{C_N}& {}^G{\tilde{p}}_{C_N}^T\end{array}\right]}^T\text{\hspace{1em}(2)}$$

其次整个框架需要求解的变量其实还是机体的位姿信息：
$${\hat{\mathrm{X}}}_{\mathrm{k}}={\left[\begin{array}{cccccc}{\hat{\mathrm{X}}}_{\mathrm{I}\mathrm{M}\mathrm{U}}& {}_G^{C_1}{\hat{\overline{q}}}^T& {}^G{\hat{p}}_{C_1}^T& ...& {}_G^{C_N}{\hat{\overline{q}}}^T& {}^G{\hat{p}}_{C_N}^T\end{array}\right]}^T\text{\hspace{1em}(3)}$$
 

---

MSCKF的预测部分

这里简单回顾MSCKF的预测部分，详细可以阅读参考[6]；

IMU误差状态的传导

对于每一帧IMU数据，都可以根据误差状态的微分方程求得状态转移部分的递推方程的闭式解，如下：
$$\begin{array}{r}\{\begin{array}{ll}{\tilde{\mathrm{x}}}_{\mathrm{k}+1}^{\mathrm{I}\mathrm{M}\mathrm{U}}& =\Phi (\mathrm{k}+1,\mathrm{k}){\tilde{\mathrm{x}}}_{\mathrm{k}}^{\mathrm{I}\mathrm{M}\mathrm{U}}\\ {\mathrm{P}}_{\mathrm{k}+1|\mathrm{k}}^{\mathrm{I}\mathrm{M}\mathrm{U}}& =\Phi (\mathrm{k}+1,\mathrm{k}){\mathrm{P}}_{\mathrm{k}|\mathrm{k}}^{\mathrm{I}\mathrm{M}\mathrm{U}}\Phi (\mathrm{k}+1,\mathrm{k}{)}^T+\Phi (\mathrm{k}+1,\mathrm{k})\mathrm{G}\mathrm{N}{\mathrm{G}}^T\Phi (\mathrm{k}+1,\mathrm{k}{)}^T\end{array}\end{array}\text{\hspace{1em}(4)}$$
 

新帧到达前的协方差矩阵更新

新帧未到来之前，由于没有任何新的观测可以影响相机位姿，因此整个协方差的更新过程如下：
$${\mathrm{P}}_{k+1|k}=\left[\begin{array}{cc}{\mathrm{P}}_{k+1|k}^{IMU}& \Phi (k+1,k){\mathrm{P}}_{k|k}^{IC}\\ (\Phi (k+1,k){\mathrm{P}}_{k|k}^{IC}{)}^T& {\mathrm{P}}_{k|k}^{CAM}\end{array}\right]\text{\hspace{1em}(5)}$$
 

新帧到来时的协方差矩阵的扩展

新帧到来的时候，算法直接使用外参矩阵讲IMU的最新位姿作为其位姿，因此两者的误差状态满足：
$${\tilde{\mathrm{X}}}_{\mathrm{k}+1|\mathrm{k}}^{\mathrm{C}\mathrm{A}\mathrm{M}}=\left[\begin{array}{c}{}_G^C\delta \theta \\ {}^G{\tilde{p}}_C\end{array}\right]=\left[\begin{array}{ccccccc}{\hat{\mathrm{R}}}_{\mathrm{I}}^{\mathrm{C}}& 0_{3x3}& 0_{3x3}& 0_{3x3}& 0_{3x3}& {\mathbf{I}}_{3x3}& 0_{3x3}\\ -{{\hat{\mathrm{R}}}_{\mathrm{G}}^{\mathrm{I}}}^T([{}^I{\hat{\mathrm{p}}}_{\mathrm{C}}{]}_{\times })& 0_{3x3}& 0_{3x3}& 0_{3x3}& {\mathbf{I}}_{3x3}& 0_{3x3}& {{\hat{\mathrm{R}}}_{\mathrm{G}}^{\mathrm{I}}}^T\end{array}\right]\left[\begin{array}{c}{}_G^I\delta \theta \\ {\tilde{b}}_g\\ {}^G{\tilde{v}}_I\\ {\tilde{b}}_a\\ {}^G{\tilde{p}}_I\\ {}_I^C\delta \theta \\ {}^I{p}_C\end{array}\right]={\mathbf{J}}_{IMU}^{CAM}{\tilde{\mathrm{X}}}_{\mathrm{k}+1|\mathrm{k}}^{\mathrm{I}\mathrm{M}\mathrm{U}}\text{\hspace{1em}(6)}$$
所以整个的协方差矩阵的扩展过程如下：
$${\mathbf{P}}_{k+1\mid k}\leftarrow \left[\begin{array}{c}{\mathbf{I}}_{6N+21}\\ \mathbf{J}\end{array}\right]{\mathbf{P}}_{k+1\mid k}{\left[\begin{array}{c}{\mathbf{I}}_{6N+21}\\ \mathbf{J}\end{array}\right]}^T\text{\hspace{1em}(7)}$$

 

---

MSCKF更新部分

该部分主要探讨新帧的观测是如何影响整个滤波的。

何时进行更新

根据参考1中III-E节，MSCKF在两个事件上进行更新操作：

1. 当一个特征点被跟踪丢失的时候，此时特征点的信息不再变化，且此时进行三角化的时候，相机位姿的信息也均是优化多次的位姿，较为准确，我们称之为情况1；
2. 当窗口中的相机位姿达到上限的时候，此时势必要把一帧旧的相机位姿滑动出去，此时算法会将所有与该位姿相关的特征点用于更新，最大程度的再次利用这个位姿，我们称之为情况2；

在S-MSCKF中，上面的两个部分对应两个函数：

1. removeLostFeatures();
2. pruneCamStateBuffer();

后面会简单看一下实际工程中作者是如何做的。

 

单个特征点的观测方程

在MSCKF中，如果一个点在k时刻被跟丢了，那么该特征点会用来更新整个看到它的相机位姿。

假设特征点$f_j$在世界坐标系下的位置为${}^G{P}_{f_j}$，于是对于相机$i$，可以通过位姿将其转换到相机坐标系下：
$${}^{C_i}{\mathbf{p}}_{f_j}=\left[\begin{array}{c}{}^{C_i}{X}_j\\ {}^{C_i}{Y}_j\\ {}^{C_i}{Z}_j\end{array}\right]=\mathbf{C}\left({}_G^{C_i}\bar{q}\right)\left({}^G{\mathbf{p}}_{f_j}-{}^G{\mathbf{p}}_{C_i}\right)\text{\hspace{1em}(8)}$$
于是对应的观测就是：
$${\mathbf{z}}_i^{(j)}=\frac{1}{{}^{C_i}{Z}_j}\left[\begin{array}{c}{}^{C_i}{X}_j\\ {}^{C_i}{Y}_j\end{array}\right]+{\mathbf{n}}_i^{(j)},i\in {\mathcal{S}}_j\text{\hspace{1em}(9)}$$
但是到这个地方其实还并不是MSCKF的观测方程，因为MSCKF的估计变量为error-state，所以进一步定义误差：
$$\begin{array}{rl}{r}_i^{(j)}& =z_i^{(j)}-{\hat{z}}_i^{(j)}({\mathrm{x}}_{\mathrm{i}})\\ & =z_i^{(j)}-{\hat{z}}_i^{(j)}({\hat{\mathrm{x}}}_{\mathrm{i}}+{\tilde{\mathrm{x}}}_{\mathrm{i}})\\ & =z_i^{(j)}-({\hat{z}}_i^{(j)}({\hat{\mathrm{x}}}_{\mathrm{i}})+{\mathrm{J}}_{{\hat{\mathrm{x}}}_{\mathrm{i}}}^{(\mathrm{j})}{\tilde{\mathrm{x}}}_{\mathrm{i}}+{\mathrm{J}}_{{}^{\mathrm{G}}{\hat{\mathrm{p}}}_{{\mathrm{f}}_{\mathrm{j}}}}^{(\mathrm{j})}{}^{\mathrm{G}}{\tilde{\mathrm{p}}}_{{\mathrm{f}}_{\mathrm{j}}})\\ & ={\mathrm{H}}_{{\hat{\mathrm{x}}}_i}^{(j)}{\tilde{\mathrm{x}}}_{\mathrm{i}}+{\mathrm{H}}_{{\hat{p}}_{f_j}}^{(j)}{}^{\mathrm{G}}{\tilde{\mathrm{p}}}_{{\mathrm{f}}_{\mathrm{j}}}+{\mathbf{n}}_i^{(j)}\end{array}\text{\hspace{1em}(10)}$$
上面的公式（10）就是我们需要的对于error-state的观测公式了；

 

对于观测方程的化简

这一步主要是对观测方程进行变换，主要有两个目的：

1. 将特征点的优化从状态空间中去掉，这样整个状态空间中估计的就只有相机的位姿，大大减少待估计的变量数；
2. 由于MSCKF对特征点采用的是延迟初始化的策略，因此当该特征点被三角化的时候，其值已经足够精准，且如果是情况1的话，这个优化显得费力不讨好；

对于特征点$f_j$而言，假设所有观测到该特征点的相机为${\mathrm{x}}_i,i\in [1,N]$，根据公式（10），那么一共有N个residual可以叠加在一起，于是有：
$${\mathbf{r}}^{(j)}\simeq {\mathbf{H}}_{\mathbf{X}}^{(j)}\tilde{\mathbf{X}}+{\mathbf{H}}_f^{(j)G}{\tilde{\mathbf{p}}}_{f_j}+{\mathbf{n}}^{(j)}\text{\hspace{1em}(11)}$$
其中：

1. residual的维度为2N x 1；
2. ${\mathbf{H}}_{\mathbf{X}}^{(j)}$维度为2N x 5M，其中M为滑动窗口中所有的相机数；
3. ${\mathbf{H}}_{\mathbf{f}}^{(j)}$维度为2N x 3；

容易看到，当N大于2时，${\mathbf{H}}_f^{(j)}$的秩为3（这里不用过分纠结有没有可能是秩<=3，简单说，只要有重投影误差，那么秩就一定>=3），于是其左零空间的维度为（2N-3），假设为矩阵${\mathbf{A}}^{\mathbf{T}}$（左零一定加转置符号哈），其维度为（2N-3 x 2N）所以公式（11）两端均乘以矩阵${\mathbf{A}}^{\mathbf{T}}$得到：
$$\begin{array}{rl}{\mathbf{r}}_o^{(j)}={\mathbf{A}}^{\mathbf{T}}{\mathbf{r}}^{(j)}& \simeq {\mathbf{A}}^{\mathbf{T}}{\mathbf{H}}_{\mathbf{X}}^{(j)}\tilde{\mathbf{X}}+{\mathbf{A}}^{\mathbf{T}}{\mathbf{H}}_f^{(j)G}{\tilde{\mathbf{p}}}_{f_j}+{\mathbf{A}}^{\mathbf{T}}{\mathbf{n}}^{(j)}\\ & ={\mathbf{H}}_o^{(j)}\tilde{\mathbf{X}}+{\mathbf{n}}_o^{(j)}\end{array}\text{\hspace{1em}(12)}$$
其中：

1. 上式中所有矩阵或者向量的行数均为（2N-3）；

2. 噪声矩阵的协方差满足：
   $$E\left\{{\mathbf{n}}_o^{(j)}{\mathbf{n}}_o^{(j)T}\right\}={\sigma }_{\mathrm{i}\mathrm{m}}^2{\mathbf{A}}^T\mathbf{A}={\sigma }_{\mathrm{i}\mathrm{m}}^2{\mathbf{I}}_{2M_j-3}\text{\hspace{1em}(13)}$$

 

对于观测方程的进一步简化

如公式（12）所示，对于一个特征点而言，该公式的计算维度为$（2{\mathrm{N}}^{(j)}-3）$，那么对于所有的点而言，该问题的维度为$d=\sum (2{\mathrm{N}}^{(j)}-3)$。

特别的，如果该维度大于估计变量（error-state）的维度时，多出来的维度的优化显得没有必要，所以，当${\mathbf{H}}_{\mathbf{o}}$的行数大于列数的时候，作者引入了QR分解来帮助进行优化问题的再简化：
$${\mathbf{H}}_{\mathbf{o}}=\left[Q_1,Q_2\right]\left[\begin{array}{c}{\mathbf{T}}_{\mathbf{H}}\\ 0\end{array}\right]\text{\hspace{1em}(14)}$$
因为Q矩阵是酉矩阵，所以对于公式（12），可以变为：
$$\begin{array}{rl}\left[\begin{array}{c}{Q}_1^T{\mathbf{r}}_{\mathbf{o}}^{(\mathbf{j})}\\ Q_2^T{\mathbf{r}}_{\mathbf{o}}^{(\mathbf{j})}\end{array}\right]& =\left[\begin{array}{c}{\mathbf{T}}_{\mathbf{H}}^{(\mathbf{j})}\\ 0\end{array}\right]\tilde{\mathbf{X}}+\left[\begin{array}{c}{Q}_1^T{\mathbf{n}}_{\mathbf{o}}^{(\mathbf{j})}\\ Q_2^T{\mathbf{n}}_{\mathbf{o}}^{(\mathbf{j})}\end{array}\right]\\ \Rightarrow {\mathbf{r}}_{\mathbf{n}}^{(\mathbf{j})}& ={\mathbf{T}}_{\mathbf{H}}^{(\mathbf{j})}\tilde{\mathbf{X}}+{\mathbf{n}}_{\mathbf{n}}^{(\mathbf{j})}\end{array}\text{\hspace{1em}(15)}$$
其中：

1. ${\mathbf{T}}_{\mathbf{H}}$的维度为$(6\mathrm{M}+21)\times (6\mathrm{M}+21)$，M为滑动窗口中的相机位姿数目；

2. 对于噪声部分，依旧引用公式（13），依旧可以看到：
   $$E\left\{{\mathbf{n}}_n^{(j)}{\mathbf{n}}_n^{(j)T}\right\}={\sigma }_{\mathrm{i}\mathrm{m}}^2{\mathbf{Q}}^T\mathbf{Q}={\sigma }_{\mathrm{i}\mathrm{m}}^2{\mathbf{I}}_{6M+21}\text{\hspace{1em}(16)}$$

 

更新方程

有了公式（15）所示的观测方程之后，其实就可以套入KF的更新方程了：
$$\{\begin{array}{l}\mathbf{K}={\mathbf{P}{\mathbf{T}}_{\mathbf{H}}^{\mathbf{T}}({\mathbf{T}}_{\mathbf{H}}\mathbf{P}{\mathbf{T}}_{\mathbf{H}}^{\mathbf{T}}+{\mathbf{R}}_{\mathbf{n}})}^{-1}\\ \mathbf{\Delta }\mathbf{X}=\mathbf{K}{\mathbf{r}}_{\mathbf{n}}\\ {\mathbf{P}}_{k+1|k+1}=(\mathbf{I}-\mathbf{K}{\mathbf{T}}_{\mathbf{H}}){\mathbf{P}}_{k+1|k}(\mathbf{I}-\mathbf{K}{\mathbf{T}}_{\mathbf{H}}{)}^T+\mathbf{K}{\mathbf{R}}_{\mathbf{n}}{\mathbf{K}}^{\mathbf{T}}\end{array}\text{\hspace{1em}(17)}$$
可以看到，基本上符合整个KF的更新过程，但是第二个公式和传统的KF的方程似乎不太一样，其实本质上两者是一样的，首先看传统下的KF的计算方程：
$${\mathbf{X}}_{k+1|k+1}={\mathbf{X}}_{k+1|k}+\mathbf{K}(\mathrm{z}-\mathrm{z}({\mathbf{X}}_{k+1|k}))\text{\hspace{1em}(18)}$$
对于MSCKF：

1. 由于估计的变量为error-state，所以此时的${\mathbf{X}}_{k+1|k}=0$；

2. 再看观测误差项，这里回溯到公式（10）就可以看出，在${\tilde{\mathbf{X}}}_{k+1|k}=0$的前提下：
   $$r_i^{(j)}=z_i^{(j)}-({\hat{z}}_i^{(j)}({\hat{\mathrm{x}}}_{\mathrm{i}})+{\mathrm{J}}_{{\hat{\mathrm{x}}}_{\mathrm{i}}}^{(\mathrm{j})}{\tilde{\mathrm{x}}}_{\mathrm{i}}+{\mathrm{J}}_{{}^{\mathrm{G}}{\hat{\mathrm{p}}}_{{\mathrm{f}}_{\mathrm{j}}}}^{(\mathrm{j})}{}^{\mathrm{G}}{\tilde{\mathrm{p}}}_{{\mathrm{f}}_{\mathrm{j}}})=z_i^{(j)}-({\hat{z}}_i^{(j)}({\hat{\mathrm{x}}}_{\mathrm{i}})+{\mathrm{J}}_{{\hat{\mathrm{x}}}_{\mathrm{i}}}^{(\mathrm{j})}0+{\mathrm{J}}_{{}^{\mathrm{G}}{\hat{\mathrm{p}}}_{{\mathrm{f}}_{\mathrm{j}}}}^{(\mathrm{j})}0)=z_i^{(j)}-{\hat{z}}_i^{(j)}({\hat{\mathrm{x}}}_{\mathrm{i}})\text{\hspace{1em}(19)}$$

所以我们看到公式（17）是符合整个KF更新的；

 

更新方程之后

根据公式（17）计算完整个更新量之后，就可以使用更新方程对normal-state进行更新了。同时注意要reset掉k时刻的估计值，这个地方一般情况下不必对协方差矩阵进行再次的计算，但是实际上，在参考2的第6章中的介绍中，在reset的时候也是需要进行协方差矩阵进行稍微的更新的，只不过一般情况下，更新矩阵近似为单位阵，所以不需要更新。

 

---

疑惑——观测如何影响IMU的参数的

至此其实整个MSCKF的整个过程都已经整理完毕了，但是笔者一直有一个疑惑：视觉的观测是如何影响IMU参数的？

1. 当删除跟踪丢失的特征点时，因为当前帧也就是k时刻的帧已经没有了观测了，所以观测矩阵中对应项为0，那么观测是如何传导到IMU的参数的？
2. 当删除旧的关键帧的时候，会把与其关联的所有的特征点拿出来进行更新，这其中一定有些特征点会被当前帧观测到（当前帧观测不到的会被直接删掉了），进而产生约束；

综上，笔者开始的时候认为只有当删除旧的关键帧的时候，IMU的参数才会被更新。

直到…笔者将S-MSCKF中的对应的H矩阵以及error-state的更新量打印出来才发现，事情并没有那么简单：其实在删除跟踪丢失的关键点的时候，IMU的参数部分也会变化。但是H矩阵符合预期，在最新帧的部分全为0。

实际上对于MSCKF（或者说对于KF这样的方法）而言，真正链接IMU和之前的状态的因素除了观测的约束，还有一个部分就是协方差。

这里简单的记录一下个人的理解：为了方便理解，这里直接用Kalman filter的bayes推导方法

假设在k时刻有了新的输入值${\mathrm{v}}_k$和新的观测值${\mathrm{y}}_k$；

于是k时刻的状态变量的预测值为：
$$p\left({\mathbf{x}}_k\mid {\check{\mathbf{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k-1}\right)=\mathcal{N}\left({\check{\mathbf{x}}}_k,{\check{\mathbf{P}}}_k\right)\text{\hspace{1em}(20)}$$
其中：
$$\{\begin{array}{l}\begin{array}{rl}{\check{\mathbf{P}}}_k& ={\mathbf{A}}_{k-1}{\hat{\mathbf{P}}}_{k-1}{\mathbf{A}}_{k-1}^T+{\mathbf{Q}}_k\\ {\check{\mathbf{x}}}_k& ={\mathbf{A}}_{k-1}{\hat{\mathbf{x}}}_{k-1}+{\mathbf{v}}_k\end{array}\end{array}\text{\hspace{1em}(21)}$$
对于k时刻的观测值${\mathrm{y}}_k$，给出其与状态变量的联合概率分布：
$$\begin{array}{rl}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\\ {\mathbf{\mu }}_y\end{array}\right],\left[\begin{array}{cc}{\mathbf{\Sigma }}_{xx}& {\mathbf{\Sigma }}_{xy}\\ {\mathbf{\Sigma }}_{yx}& {\mathbf{\Sigma }}_{yy}\end{array}\right]\right)\\ & =\mathcal{N}\left(\left[\begin{array}{c}{\check{\mathbf{x}}}_k\\ {\mathbf{C}}_k{\check{\mathbf{x}}}_k\end{array}\right],\left[\begin{array}{cc}{\tilde{\mathbf{P}}}_k& {\check{\mathbf{P}}}_k{\mathbf{C}}_k^T\\ {\mathbf{C}}_k{\mathbf{P}}_k& {\mathbf{C}}_k{\mathbf{P}}_k{\mathbf{C}}_k^T+{\mathbf{R}}_k\end{array}\right]\right)\end{array}\text{\hspace{1em}(22)}$$
对公式（22）所表示的概率对观测进行边缘化得到：
$$p({\mathrm{x}}_k|{\check{\mathrm{x}}}_0,{\mathbf{v}}_{1:k},{\mathbf{y}}_{0:k})=\mathcal{N}(\underset{{\hat{\mathbf{x}}}_k}{\underbrace{{\mathbf{\mu }}_x+{\mathbf{\Sigma }}_{xy}{\mathbf{\Sigma }}_{yy}^{-1}\left({\mathbf{y}}_k-{\mathbf{\mu }}_y\right)}},\underset{{\hat{\mathbf{P}}}_k}{\underbrace{{\mathbf{\Sigma }}_{xx}-{\mathbf{\Sigma }}_{xy}{\mathbf{\Sigma }}_{yy}^{-1}{\mathbf{\Sigma }}_{yx}}})\text{\hspace{1em}(23)}$$
公式（23）中的${\Sigma }_{xy}{\Sigma }_{yy}^{-1}$对应的就是KF中的增益矩阵K。

可以看到，联合概率分布中，观测与状态变量的协方差${\Sigma }_{xy}$既与观测矩阵C有关，同时也与协方差矩阵P有关。

对于MSCKF框架而言，在滑动窗口中的相机位姿与IMU参数的协方差其实一直都不为0，理论上来说，每一帧相机位姿其实都来源于IMU，所以两者之间必然是联系起来的。

 

---

总结

本文总结了MSCKF的更新过程，整体上算法还是在按照KF的思路（更确切的说在按照ESKF的思路）在走，整个过程也比较好理解。

下一个专题主要想深入的分析一下整个优化过程可观性的分析，算是更深入的对优化问题中的细节部分进行理解。