学习MSCKF笔记——后端、状态预测、状态扩增、状态更新

学习MSCKF笔记——后端、状态预测、状态扩增、状态更新

为了看懂后端代码，我先看了下《Quaternion kinematics for the error-state Kalman filter》这篇参考文献，写了两篇总结文档
学习MSCKF笔记——四元数基础
学习MSCKF笔记——真实状态、标称状态、误差状态
MSCKF的后端内容还是很多的，Stereo-MSCKF的代码也写得很好，通过读代码将MSCKF后端流程图总结如下:

下面我将后端中几个关键的知识点相关的理论推导和代码进行总结分析。

1. 状态预测

首先我们要清楚的一点是，在MSCKF中维护的状态变量是由两部分组成，分别是IMU误差状态${\mathbf{x}}_I={\left[\delta {\mathbf{\theta }}_{\mathrm{i}2\mathrm{w}}^T,\delta {\mathbf{b}}_g^T,{\delta }^{\mathrm{w}}{\mathbf{v}}^T,\delta {\mathbf{b}}_a^T,{\delta }^{\mathrm{w}}{\mathbf{p}}_i^T,\delta {\mathbf{\theta }}_{\mathrm{i}2\mathrm{c}}^T,\delta {\mathbf{t}}_{\mathrm{i}2\mathrm{c}}^T\right]}^T$和相机误差状态${\mathbf{x}}_{c_{}}={\left[\delta {\mathbf{\theta }}_{w2c_{}}^T,{\delta }^{\mathrm{w}}{\mathbf{p}}_{c_{}}^T\right]}^T$，在状态预测过程中，只是对IMU的标称状态和IMU的误差状态进行预测，预测的过程中会改变IMU误差状态的协方差以及IMU误差状态相对相机误差状态的协方差。而相机相关的状态只会在观测更新时改变。
在学习MSCKF笔记——误差状态卡尔曼滤波中，我们讨论了IMU的标称状态和误差状态分别的定义和推导，下面我们就可以具体看看，在MSCKF中是如何实际应用这两种状态变量的，对于标称状态，与上一篇博文中不同的是，MSCKF中IMU的标称状态采用的是四阶龙格库塔积分，而不是欧拉积分，精度会更高，四阶龙格库塔不展开进行讨论，我们着重讨论下IMU误差状态的实际应用。
在上一篇博客我们对IMU的误差状态的连续时间系统进行了推导，并进行了近似离散化（精确离散化方法的一阶泰勒展开），这里我们首先给出IMU误差状态连续时间系统方程（这里省略下标$I$）$$\dot{\mathbf{x}}(t)=\mathbf{F}(t)\mathbf{x}(t)+\mathbf{G}(t)\mathbf{w}(t)$$其中状态变量为$\mathbf{x}={\left[\delta {\mathbf{\theta }}_{\mathrm{i}2\mathrm{w}}^T,\delta {\mathbf{b}}_g^T,{\delta }^{\mathrm{w}}{\mathbf{v}}^T,\delta {\mathbf{b}}_a^T,{\delta }^{\mathrm{w}}{\mathbf{p}}_i^T,\delta {\mathbf{\theta }}_{\mathrm{i}2\mathrm{c}}^T,\delta {\mathbf{t}}_{\mathrm{i}2\mathrm{c}}^T\right]}^T$，噪声项为$\mathbf{w}={\left[\begin{array}{l}{\mathbf{\omega }}_n^T,{\mathbf{\omega }}_{\omega }^T,{\mathbf{a}}_n^T,{\mathbf{a}}_{\omega }^T\end{array}\right]}^T$其中噪声项的均值和方差分别为$E[\mathbf{n}(t)]=0,E\left[\mathbf{n}(t{)}^T\mathbf{n}(\tau )\right]=\mathbf{q}\delta (t-\tau )$$$\underset{(21\times 21)}{F}=\left[\begin{array}{ccccccc}-{\left[{\omega }_m-{\omega }_b\right]}_{\times }& -\mathbf{I}& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\\ -{\mathbf{R}}_{\mathrm{i}2\mathrm{w}}{\left[{\mathbf{a}}_m-{\mathbf{a}}_b\right]}_{\times }& 0& 0& -{\mathbf{R}}_{\mathrm{i}2\mathrm{w}}& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\\ 0& 0& \mathbf{I}& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0\end{array}\right]$$$$\underset{(21\times 12)}{G}=\left[\begin{array}{cccc}-\mathbf{I}& 0& 0& 0\\ 0& \mathbf{I}& 0& 0\\ 0& 0& -{\mathbf{R}}_{\mathrm{i}2\mathrm{w}}& 0\\ 0& 0& 0& \mathbf{I}\\ 0& 0& 0& 0\\ 0& 0& 0& 0\\ 0& 0& 0& 0\end{array}\right]$$由于上一篇博客中对IMU的误差状态的连续时间系统进行了推导，唯一不同的是在MSCKF的误差状态中考虑相机和IMU之间的旋转$\delta {\mathbf{\theta }}_{\mathrm{i}2\mathrm{c}}^T$和平移$\delta {\mathbf{t}}_{\mathrm{i}2\mathrm{c}}^T$，对应的代码部分如下：

  // Compute discrete transition and noise covariance matrix
  Matrix<double, 21, 21> F = Matrix<double, 21, 21>::Zero(); 
  Matrix<double, 21, 12> G = Matrix<double, 21, 12>::Zero();

  F.block<3, 3>(0, 0) = -skewSymmetric(gyro);
  F.block<3, 3>(0, 3) = -Matrix3d::Identity();
  F.block<3, 3>(6, 0) = -quaternionToRotation(
      imu_state.orientation).transpose()*skewSymmetric(acc);
  F.block<3, 3>(6, 9) = -quaternionToRotation(
      imu_state.orientation).transpose();
  F.block<3, 3>(12, 6) = Matrix3d::Identity();

  G.block<3, 3>(0, 0) = -Matrix3d::Identity();
  G.block<3, 3>(3, 3) = Matrix3d::Identity();
  G.block<3, 3>(6, 6) = -quaternionToRotation(
      imu_state.orientation).transpose();
  G.block<3, 3>(9, 9) = Matrix3d::Identity();

获得误差状态的连续时间系统推导公式后，我们对其进行离散化，根据线性系统理论，连续时间系统离散化形式为：$$\mathbf{x}\left({\mathrm{t}}_{k+1}\right)=\mathbf{\Phi }\left(t_{k+1},t_k\right)\mathbf{x}\left({\mathrm{t}}_k\right)+{\int }_{t_k}^{t_{t+1}}\mathbf{\Phi }\left(t_{k+1},\tau \right)\mathbf{G}(\tau )\mathbf{w}(\tau )d\tau$$我们将短间隔时间内的$\mathbf{F}(t)$矩阵作为常阵${\mathbf{F}}_k$处理并对精确离散化公式进行三次泰勒展开，获得$$\mathbf{\Phi }\left(t_{k+1},t_k\right)={\mathrm{e}}^{\mathrm{F}\Delta t}=\mathbf{I}+\mathbf{F}\Delta t+\frac{1}{2}(\mathbf{F}\Delta t{)}^2+\frac{1}{3!}(\mathbf{F}\Delta t{)}^3+O\left(\Vert \mathbf{F}\Delta t{\Vert }^4\right)$$同时，我们令$${\mathbf{W}}_k={\int }_{t_k}^{t_{t+1}}\mathbf{\Phi }\left(t_{k+1},\tau \right)\mathbf{G}(\tau )\mathbf{w}(\tau )d\tau$$则离散化后即获得运动方程为$${\mathbf{x}}_{k+1}={\mathbf{\Phi }}_{k+1\mid k}{\mathbf{x}}_k+{\mathbf{W}}_k$$我们计算噪声项${\mathbf{W}}_k$的均值和方差分别为$E\left[{\mathbf{W}}_k\right]=0,E\left[{\mathbf{W}}_k{\mathbf{W}}_k^T\right]={\mathbf{Q}}_k\delta (t-\tau )$，其中$${\mathbf{Q}}_k={\int }_{t_k}^{t_{k+1}}\mathbf{\Phi }\left(t_{k+1},t\right)\mathbf{G}(t)\mathbf{q}\mathbf{G}(t{)}^T{\mathbf{\Phi }}^T\left(t_{k+1},t\right)dt$$
这一部分代码对应为：

  Matrix<double, 21, 21> Fdt = F * dtime;
  Matrix<double, 21, 21> Fdt_square = Fdt * Fdt;
  Matrix<double, 21, 21> Fdt_cube = Fdt_square * Fdt;
  Matrix<double, 21, 21> Phi = Matrix<double, 21, 21>::Identity() +
    Fdt + 0.5*Fdt_square + (1.0/6.0)*Fdt_cube;
  Matrix<double, 21, 21> Q = Phi*G*state_server.continuous_noise_cov*
    G.transpose()*Phi.transpose()*dtime;

我们求得运动方程的的噪声项方差的主要目的是进行卡尔曼滤波的运动学推导，我们可以不管误差状态的均值，因为误差状态是噪声驱动的，噪声项的均值为零，在运动方程的迭代下均值始终为零，而误差状态的方差则会收到噪声方差影响：$${\mathbf{P}}_{I{I}_{k+1\mid k}}={\mathbf{\Phi }}_{k+1\mid k}{\mathbf{P}}_{I{I}_{k\mid k}}{\mathbf{\Phi }}_{k+1\mid k}^T+{\mathbf{Q}}_k$$同时，如果有相机状态存在时，由于IMU的误差状态发生了改变，那么相机误差状态相对于IMU误差状态的协方差也会发生改变，对应的代码如下：

  state_server.state_cov.block<21, 21>(0, 0) =
    Phi*state_server.state_cov.block<21, 21>(0, 0)*Phi.transpose() + Q;
  if (state_server.cam_states.size() > 0) {
    state_server.state_cov.block(
        0, 21, 21, state_server.state_cov.cols()-21) =
      Phi * state_server.state_cov.block(
        0, 21, 21, state_server.state_cov.cols()-21);
    state_server.state_cov.block(
        21, 0, state_server.state_cov.rows()-21, 21) =
      state_server.state_cov.block(
        21, 0, state_server.state_cov.rows()-21, 21) * Phi.transpose();
  }

2. 状态扩增

注意到后端的流程是，当接收到一个图像帧之后会将当前帧与上一帧之间的所有IMU数据拿出来进行状态预测，在完成所有IMU数据的状态预测后，紧接着就是状态扩增，对于标称状态，直接将当前帧时刻IMU状态的标称状态拿来当做当前帧相机的标称状态，对应代码如下：

  Matrix3d R_w_i = quaternionToRotation(  
      state_server.imu_state.orientation);
  Matrix3d R_w_c = R_i_c * R_w_i; 
  Vector3d t_c_w = state_server.imu_state.position +
    R_w_i.transpose()*t_c_i;  

  state_server.cam_states[state_server.imu_state.id] =
    CAMState(state_server.imu_state.id);  
  CAMState& cam_state = state_server.cam_states[
    state_server.imu_state.id];

  cam_state.time = time;
  cam_state.orientation = rotationToQuaternion(R_w_c);
  cam_state.position = t_c_w;

对于误差状态，均值没啥好扩增的，因为在观测发生前，所有误差状态的均值都为零，我们最为关心的应该是误差状态的协方差矩阵扩增，这里借用吴博在深蓝学院课程上将的概念，矩阵扩增分为理解为克隆和修正两步，第一步克隆如下：$$\left[{\mathbf{x}}_i\right]⟶\left[\begin{array}{l}{\mathbf{x}}_i\\ {\mathbf{x}}_i\end{array}\right]$$$$\left[{\mathbf{P}}_i\right]⟶\left[\begin{array}{cc}{\mathbf{P}}_{ii}& {\mathbf{P}}_{ii}\\ {\mathbf{P}}_{ii}& {\mathbf{P}}_{ii}\end{array}\right]$$第二步修正如下：$$\left[{\mathbf{x}}_i\right]⟶\left[\begin{array}{l}{\mathbf{x}}_i\\ {\mathbf{x}}_j\end{array}\right]$$$${\mathbf{x}}_j={\mathbf{J}}_{ji}{\mathbf{x}}_i$$$$\left[\begin{array}{ll}{\mathbf{P}}_{ii}& {\mathbf{P}}_{ii}\\ {\mathbf{P}}_{ii}& {\mathbf{P}}_{ii}\end{array}\right]⟶\left[\begin{array}{ll}{\mathbf{P}}_{ii}& {\mathbf{P}}_{ji}\\ {\mathbf{P}}_{ij}& {\mathbf{P}}_{jj}\end{array}\right]$$$${\mathbf{P}}_{ij}={\mathbf{J}}_{ji}{\mathbf{P}}_{ii}$$$${\mathbf{P}}_{jj}={\mathbf{J}}_{ji}{\mathbf{P}}_{ii}{\mathbf{J}}_{ji}^T$$其中${\mathbf{J}}_{ji}$为新增状态变量${\mathbf{x}}_j$相对${\mathbf{x}}_i$的雅克比矩阵，那么在MSCKF中，新增误差相机状态对当前误差状态的雅克比为$${\mathbf{J}}_{(6,21+6\mathrm{N})}={\left[{\left(\frac{\partial {\mathbf{x}}_c}{\partial {\mathbf{x}}_I^T}\right)}^T,{\left(\frac{\partial {\mathbf{x}}_c}{\partial {\mathbf{x}}_c^T}\right)}^T\right]}^T$$其中第二项是当前相机误差状态变量相对其他误差相机状态变量的的雅克比，为零矩阵，第一项为新增相机误差状态变量相对IMU误差状态变量的雅克比，结果如下（这一部分是参考吴博深蓝课程上的推导，我感觉推导过程中还有些问题和代码对应也不是完全一致，这里后续修改）：$$\frac{\partial {\mathbf{x}}_c}{\partial {\mathbf{x}}_I^T}=\left[\begin{array}{ccccc}\frac{\partial \delta {\mathbf{\theta }}_{c2w_{}}}{\partial \delta {\mathbf{\theta }}_{\mathrm{i}2\mathrm{w}}}& 0_{3\times 9}& 0_{3\times 3}& \frac{\partial \delta {\mathbf{\theta }}_{c2w_{}}}{\partial \delta {\mathbf{\theta }}_{\mathrm{i}2\mathrm{c}}}& 0_{3\times 3}\\ \frac{\partial {\delta }^{\mathrm{w}}{\mathbf{p}}_{c_{}}}{\partial \delta {\mathbf{\theta }}_{\mathrm{i}2\mathrm{w}}}& 0_{3\times 9}& \frac{\partial {\delta }^{\mathrm{w}}{\mathbf{p}}_{c_{}}}{{\delta }^{\mathrm{w}}{\mathbf{p}}_{i_{}}}& 0_{3\times 3}& \frac{\partial {\delta }^{\mathrm{w}}{\mathbf{p}}_{c_{}}}{\delta {\mathbf{t}}_{\mathrm{i}2\mathrm{c}}}\end{array}\right]$$
我们已知：$$\{\begin{array}{l}{}^w{\mathbf{p}}_c={\mathbf{R}}_{i2w}{}^i{\mathbf{p}}_c+{}^w{\mathbf{p}}_i\\ {\mathbf{R}}_{w2c}={\mathbf{R}}_{i2c}{\mathbf{R}}_{w2i}\end{array}$$
对于$\frac{\partial \delta {\mathbf{\theta }}_{c2w_{}}}{\partial \delta {\mathbf{\theta }}_{\mathrm{i}2\mathrm{w}}}$:$$\begin{array}{l}{\mathbf{R}}_{c2w}^T={\mathbf{R}}_{i2c}{\mathbf{R}}_{i2w}^T\\ \Rightarrow {\left({\mathbf{R}}_{c2w}\mathrm{Exp}\left(\delta {\mathbf{\theta }}_{c2w}\right)\right)}^T={\mathbf{R}}_{i2c}{\left({\mathbf{R}}_{i2w}\mathrm{Exp}\left(\delta {\mathbf{\theta }}_{i2w}\right)\right)}^T\\ \Rightarrow \mathrm{Exp}\left(-\delta {\mathbf{\theta }}_{c2w}\right){\mathbf{R}}_{w2c}={\mathbf{R}}_{i2c}\mathrm{Exp}\left(-\delta {\mathbf{\theta }}_{i2w}\right){\mathbf{R}}_{w2i}\\ \Rightarrow \mathrm{Exp}\left(-\delta {\mathbf{\theta }}_{c2w}\right){\mathbf{R}}_{w2c}=\mathrm{Exp}\left({\mathrm{Adj}}_{{\mathbf{R}}_{i2c}}\left(-\delta {\mathbf{\theta }}_{i2w}\right)\right){\mathbf{R}}_{i2c}{\mathbf{R}}_{w2i}\\ \Rightarrow \mathrm{Exp}\left(-\delta {\mathbf{\theta }}_{c2w}\right)=\mathrm{Exp}\left({\mathrm{Adj}}_{{\mathbf{R}}_{i2c}}\left(-\delta {\mathbf{\theta }}_{i2w}\right)\right)\\ \Rightarrow \delta {\mathbf{\theta }}_{c2w}={\mathbf{R}}_{i2c}\delta {\mathbf{\theta }}_{i2w}\end{array}$$$$\frac{\partial \delta {\mathbf{\theta }}_{c2w_{}}}{\partial \delta {\mathbf{\theta }}_{\mathrm{i}2\mathrm{w}}}={\mathbf{R}}_{i2c}$$对于$\frac{\partial \delta {\mathbf{\theta }}_{c2w_{}}}{\partial \delta {\mathbf{\theta }}_{\mathrm{i}2\mathrm{c}}}$：$$\begin{array}{l}{\mathbf{R}}_{w2c}={\mathbf{R}}_{i2c}{\mathbf{R}}_{w2i}\\ \Rightarrow {\left({\mathbf{R}}_{c2w}\mathrm{Exp}\left(\delta {\mathbf{\theta }}_{c2w}\right)\right)}^T={\left({\mathbf{R}}_{c2i}\mathrm{Exp}\left(\delta {\mathbf{\theta }}_{c2i}\right)\right)}^T{\mathbf{R}}_{w2i}\\ \Rightarrow \mathrm{Exp}\left(-\delta {\mathbf{\theta }}_{c2w}\right){\mathbf{R}}_{w2c}=\mathrm{Exp}\left(-\delta {\mathbf{\theta }}_{c2i}\right){\mathbf{R}}_{i2c}{\mathbf{R}}_{w2i}\\ \Rightarrow \delta {\mathbf{\theta }}_{c2w}=\delta {\mathbf{\theta }}_{c2i}\end{array}$$$$\frac{\partial \delta {\mathbf{\theta }}_{c2w_{}}}{\partial \delta {\mathbf{\theta }}_{\mathrm{i}2\mathrm{c}}}=\mathbf{I}$$对于$\frac{\partial {\delta }^{\mathrm{w}}{\mathbf{p}}_{c_{}}}{\partial \delta {\mathbf{\theta }}_{\mathrm{i}2\mathrm{w}}}$:$$\begin{array}{rl}\frac{{\partial }^w{\mathbf{p}}_c}{\partial \delta {\mathbf{\theta }}_{i2w}}& =\underset{\delta \mathbf{\theta }\to 0}{\lim }\frac{\left({\mathbf{R}}_{i2w}\mathrm{Exp}(\delta \mathbf{\theta }{)}^i{\mathbf{p}}_c+{}^w{\mathbf{p}}_i\right)-\left({\mathbf{R}}_{i2w}^i{\mathbf{p}}_c+{}^w{\mathbf{p}}_i\right)}{\delta \mathbf{\theta }}\\ & =\underset{\delta \theta \to 0}{\lim }\frac{\left({\mathbf{R}}_{i2w}{\left(\mathbf{I}+[\delta \mathbf{\theta }{]}_{\times }\right)}^i{\mathbf{p}}_c+{}^w{\mathbf{p}}_i\right)-\left({\mathbf{R}}_{i2w}{\mathbf{p}}_c+{}^w{\mathbf{p}}_i\right)}{\delta \mathbf{\theta }}\\ & =\underset{\delta \mathbf{\theta }\to 0}{\lim }\frac{{\mathbf{R}}_{i2w}[\delta \mathbf{\theta }{]}_{\times }^i{\mathbf{p}}_c}{\delta \mathbf{\theta }}=-{\mathbf{R}}_{i2w}{\left[{}^i{\mathbf{p}}_c\right]}_{\times }\end{array}$$对于$\frac{\partial {\delta }^w{\mathbf{p}}_{c_{}}}{{\delta }^w{\mathbf{p}}_{i_{}}}$:$$\begin{array}{rl}\frac{\partial {\delta }^w{\mathbf{p}}_{c_{}}}{{\delta }^w{\mathbf{p}}_{i_{}}}& =\underset{{\delta }^{\prime }{\mathbf{p}}_{ϵ}\to 0}{\lim }\frac{\left({\mathbf{R}}_{i2w}\left({}^i{\mathbf{p}}_c+{\delta }^i{\mathbf{p}}_c\right)+{}^w{\mathbf{p}}_i\right)-\left({\mathbf{R}}^i{\mathbf{p}}_c+{}^w{\mathbf{p}}_i\right)}{\delta \theta }\\ & =\underset{\delta \mathbf{\theta }\to 0}{\lim }\frac{{\mathbf{R}}_{i2w}{\delta }^i{\mathbf{p}}_c}{{\delta }^i{\mathbf{p}}_c}={\mathbf{R}}_{i2w}\end{array}$$对于$\frac{\partial {\delta }^{\mathrm{w}}{\mathbf{p}}_{c_{}}}{\delta {\mathbf{t}}_{\mathrm{i}2\mathrm{c}}}$:$$\frac{{\partial }^w{\mathbf{p}}_c}{\partial {\delta }^w{\mathbf{p}}_i}=\mathbf{I}$$

这里直接对应部分的代码：

  Matrix<double, 6, 21> J = Matrix<double, 6, 21>::Zero();
  J.block<3, 3>(0, 0) = R_i_c;
  J.block<3, 3>(0, 15) = Matrix3d::Identity();
  J.block<3, 3>(3, 0) = skewSymmetric(R_w_i.transpose()*t_c_i);
  J.block<3, 3>(3, 12) = Matrix3d::Identity();
  J.block<3, 3>(3, 18) = Matrix3d::Identity();

获得雅克比之后就可以按照上面状态扩增的步骤就可以求得扩增后的协方差矩阵，对应的代码如下：

  // Rename some matrix blocks for convenience.
  const Matrix<double, 21, 21>& P11 =
    state_server.state_cov.block<21, 21>(0, 0);
  const MatrixXd& P12 =
    state_server.state_cov.block(0, 21, 21, old_cols-21);

  // Fill in the augmented state covariance.
  state_server.state_cov.block(old_rows, 0, 6, old_cols) << J*P11, J*P12;
  state_server.state_cov.block(0, old_cols, old_rows, 6) =
    state_server.state_cov.block(old_rows, 0, 6, old_cols).transpose();
  state_server.state_cov.block<6, 6>(old_rows, old_cols) =
    J * P11 * J.transpose();

3. 状态更新

状态扩增后就开始构建观测并对误差状态进行更新，也就是执行卡尔曼滤波中的后三步，状态更新的前提以及三角化等操作这里就不赘述了，直接看前面的流程图就很容易搞清楚，这其中重要的步骤就是状态观测如何构建，构建的过程其实也比较简单，这一部分推导主要参考吴博在深蓝学院课程上的讲解，通过重投影误差构建观测残差：$${\mathbf{r}}^j={\mathbf{z}}^j-{\mathbf{z}}^j$$其中$j$代表第$j$个特征点，然后将残差对状态变量进行线性化，也就是求残差对状态变量的雅克比，因为观测残差和特征点还有关系，因此我们还要求残差对特征点的雅克比：$${\mathbf{r}}^j\simeq {\mathbf{H}}^j\mathbf{x}+{\mathbf{H}}^{f_{j}w}{\mathbf{p}}_j+{\mathbf{n}}^j$$但是由于我们的状态变量中没有特征点的部分，因此最后我们还需要将残差${\mathbf{r}}^j$和状态变量雅克比${\mathbf{H}}^j$投影到特征点雅克比的零空间中去，其实也就是消元掉观测方程中特征点相关的部分：$$\begin{array}{l}{\mathbf{r}}_o^j={\mathbf{V}}^T\left({\mathbf{z}}^j-{\hat{\mathbf{z}}}^j\right)\\ \simeq {\mathbf{V}}^T{\mathbf{H}}^j\mathbf{x}+{\mathbf{V}}^T{\mathbf{H}}^{f_j}{\mathbf{p}}_j+{\mathbf{V}}^T{\mathbf{n}}^j\\ ={\mathbf{V}}^T{\mathbf{H}}^j\mathbf{x}+{\mathbf{V}}^T{\mathbf{n}}^j\\ ={\mathbf{H}}_o^j\mathbf{x}+{\mathbf{n}}_o^j\end{array}$$关于这中间的细节，我们首先看下雅克比是怎么求的。首先我们考虑世界坐标系下的第$j$个3D点${}^w{\mathbf{p}}_{f_j}=[x,y,z{]}^T$，该3D点被世界坐标系下的第$i$个双目${}^w{\mathbf{p}}_{c_{i1}},{}^w{\mathbf{p}}_{c_{i2}}$观测到，对应到归一化平面上的坐标为${\mathbf{z}}_i^j=[u,v{]}^T$，那么$${\mathbf{z}}_i^j=\left[\begin{array}{l}u\\ v\end{array}\right]=\left[\begin{array}{l}\frac{x}{z}\\ \frac{y}{z}\end{array}\right]$$第$j$个特征点与第$i$个相机构成的残差为：$${\mathbf{r}}_i^j={\mathbf{z}}_i^j-{\mathbf{z}}_i^j$$因为我们在MSCKF中使用的是双目，因此会有${\mathbf{r}}_{i1}^j$和${\mathbf{r}}_{i2}^j$两个残差，而在双目状态中我们以左目状态为准，那么残差对特征点的雅克比为$$\begin{array}{c}{\mathbf{H}}_{c_i}^{f_j}\\ 4\times 3\end{array}=\left[\begin{array}{c}\frac{\partial {\mathbf{r}}_{i1}^j}{{\partial }^w{\mathbf{p}}_{c_{i1}}}\\ \frac{\partial {\mathbf{r}}_{i2}^j}{{\partial }^w{\mathbf{p}}_{c_{i1}}}\end{array}\right]=\left[\begin{array}{c}\frac{\partial {\mathbf{r}}_{i1}^j}{{\partial }^{c_{i1}}{\mathbf{p}}_{f_j}}\frac{{\partial }^{c_{i1}}{\mathbf{p}}_{f_j}}{{\partial }^w{\mathbf{p}}_{f_j}}\\ \frac{\partial {\mathbf{r}}_{i2}^j}{{\partial }^{c_{i2}}{\mathbf{p}}_{f_j}}\frac{{\partial }^{c_{i2}}{\mathbf{p}}_{f_j}}{{\partial }^w{\mathbf{p}}_{f_j}}\end{array}\right]$$残差对相机状态的雅克比为：$$\begin{array}{c}{\mathbf{H}}_{c_i}^j\\ 4\times 6\end{array}=\left[\begin{array}{cc}\frac{\partial {\mathbf{r}}_{i1}^j}{\partial {\mathbf{R}}_{w2c_{i1}}},& \frac{\partial {\mathbf{r}}_{i1}^j}{{\partial }^w{\mathbf{p}}_{c_{i1}}}\\ \frac{\partial {\mathbf{r}}_{i2}^j}{\partial {\mathbf{R}}_{w2c_{i1}}},& \frac{\partial {\mathbf{r}}_{i1}^j}{{\partial }^w{\mathbf{p}}_{c_{i1}}}\end{array}\right]=\left[\begin{array}{c}\frac{\partial {\mathbf{r}}_{i1}^j}{{\partial }^{c_{i1}}{\mathbf{p}}_{f_j}}\frac{{\partial }^{c_{i1}}{\mathbf{p}}_{f_j}}{\partial {\mathbf{R}}_{w2c_{i1}}},\frac{\partial {\mathbf{r}}_{i1}^j}{{\partial }^{c_{i1}}{\mathbf{p}}_{f_j}}\frac{{\partial }^{c_{i1}}{\mathbf{p}}_{f_j}}{{\partial }^w{\mathbf{p}}_{c_{i1}}}\\ \frac{\partial {\mathbf{r}}_{i2}^j}{{\partial }^{c_{i2}}{\mathbf{p}}_{f_j}}\frac{{\partial }^{c_{i2}}{\mathbf{p}}_{f_j}}{\partial {\mathbf{R}}_{w2c_{i1}}},\frac{\partial {\mathbf{r}}_{i2}^j}{{\partial }^{c_{i2}}{\mathbf{p}}_{f_j}}\frac{{\partial }^{c_{i2}}{\mathbf{p}}_{f_j}}{{\partial }^w{\mathbf{p}}_{c_{i1}}}\end{array}\right]$$我们知道双目的投影方程$$\{\begin{array}{l}\mathrm{left}{:}^{c_{i1}}{\mathbf{p}}_{f_j}={\mathbf{R}}_{w2c_{i1}}\left({}^w{\mathbf{p}}_{f_j}-{}^w{\mathbf{p}}_{c_{i1}}\right)\\ \text{ right: }{}^{c_{i2}}{\mathbf{p}}_{f_j}={\mathbf{R}}_{c_{i1}2c_{i2}}\left({\mathbf{R}}_{w2c_{i1}}\left({}^w{\mathbf{p}}_{f_j}-{}^w{\mathbf{p}}_{c_{i1}}\right)\right)+{\mathbf{t}}_{c_{i1}2c_{i2}}\end{array}$$然后根据这个投影方程求解雅克比$$\frac{\partial {\mathbf{r}}_i^j}{{\partial }^c{\mathbf{p}}_{f_j}}=\left[\begin{array}{llc}\frac{\partial u}{\partial x}=\frac{1}{z},& \frac{\partial u}{\partial y}=0,& \frac{\partial u}{\partial z}=-\frac{x}{z^2}\\ \frac{\partial u}{\partial x}=0,& \frac{\partial u}{\partial y}=\frac{1}{z},& \frac{\partial u}{\partial z}=-\frac{y}{z^2}\end{array}\right]$$$$\frac{{\partial }^{c_{i1}}{\mathbf{p}}_{f_j}}{{\partial }^w{\mathbf{p}}_{f_j}}={\mathbf{R}}_{w2c_{i1}}\frac{{\partial }^{c_{i2}}{\mathbf{p}}_{f_j}}{{\partial }^w{\mathbf{p}}_{f_j}}={\mathbf{R}}_{w2c_{i2}}$$$$\frac{{\partial }^{c_{i1}}{\mathbf{p}}_{f_j}}{{\partial }^w{\mathbf{p}}_{c_{i1}}}=-{\mathbf{R}}_{w2c_{i1}}\frac{{\partial }^{c_{i2}}{\mathbf{p}}_{f_j}}{{\partial }^w{\mathbf{p}}_{c_{i1}}}=-{\mathbf{R}}_{w2c_{i2}}$$$$\begin{array}{rl}\frac{{\partial }^{c_{i1}}{\mathbf{p}}_{f_j}}{\partial {\mathbf{R}}_{w2c_{i1}}}& =\underset{\delta \mathbf{\theta }\to 0}{\lim }\frac{{\left({\mathbf{R}}_{c_{i1}2w}\mathrm{Exp}(\delta \mathbf{\theta })\right)}^T\left({}^w{\mathbf{p}}_{f_j}-{}^w{\mathbf{p}}_{c_{i1}}\right)-{\mathbf{R}}_{w2c_{i1}}\left({}^w{\mathbf{p}}_{f_j}-{}^w{\mathbf{p}}_{c_{i1}}\right)}{\delta \theta }\\ & =\underset{\delta \mathbf{\theta }\to 0}{\lim }\frac{\left(\mathbf{I}-[\delta \mathbf{\theta }{]}_x\right){\mathbf{R}}_{w2c_{i1}}\left({}^w{\mathbf{p}}_{f_j}-{}^w{\mathbf{p}}_{c_{i1}}\right)-{\mathbf{R}}_{w2c_{i1}}\left({}^w{\mathbf{p}}_{f_j}-{}^w{\mathbf{p}}_{c_{i1}}\right)}{\delta \theta }\\ & =\underset{\delta \mathbf{\theta }\to 0}{\lim }\frac{-[\delta \mathbf{\theta }{]}_{\times }{\mathbf{R}}_{w2c_{i1}}\left({}^w{\mathbf{p}}_{f_j}-{{}^w\mathbf{p}}_{c_{i1}}\right)}{\delta \mathbf{\theta }}\\ & ={\left[{\mathbf{R}}_{w2c_{i1}}\left({}^w{\mathbf{p}}_{f_j}-{}^w{\mathbf{p}}_{c_{i1}}\right)\right]}_{\times }={\left[{}^{c_{i1}}{\mathbf{p}}_{f_j}\right]}_{\times }\end{array}$$$$\begin{array}{rl}\frac{{\partial }^{c_{i2}}{\mathbf{p}}_{f_j}}{\partial {\mathbf{R}}_{w2c_{i1}}}& =\underset{\delta \mathbf{\theta }\to 0}{\lim }\frac{{\left({\mathbf{R}}_{c_{i1}2w}\mathrm{Exp}(\delta \mathbf{\theta }){\mathbf{R}}_{c_{i2}2c_{i1}}\right)}^T\left({}^w{\mathbf{p}}_{f_j}-{}^w{\mathbf{p}}_{c_{i1}}\right)-{\mathbf{R}}_{c_{i1}2c_{i2}}{\mathbf{R}}_{w2c_{i1}}\left({}^w{\mathbf{p}}_{f_j}-{}^w{\mathbf{p}}_{c_{i1}}\right)}{\delta \mathbf{\theta }}\\ & =\underset{\delta \mathbf{\theta }\to 0}{\lim }\frac{{\mathbf{R}}_{c_{i1}2c_{i2}}\left(\mathbf{I}-[\delta \mathbf{\theta }{]}_{\times }\right){\mathbf{R}}_{w2c_{i1}}\left({}^w{\mathbf{p}}_{f_j}-{}^w{\mathbf{p}}_{c_{i1}}\right)-{\mathbf{R}}_{c_{11}2c_{i2}}{\mathbf{R}}_{w2c_{i1}}\left(w_{{\mathbf{p}}_{f_j}}-w_{{\mathbf{p}}_{c_{i1}}}\right)}{\delta \mathbf{\theta }}\\ & ={\mathbf{R}}_{c_{i_1}2c_{i_2}}{\left[{\mathbf{R}}_{w2c_1}\left({}^w{\mathbf{p}}_{f_j}-{}^w{\mathbf{p}}_{c_{i1}}\right)\right]}_{\times }={\mathbf{R}}_{c_{i1}2c_{i2}}{\left[{}^{c_{i1}}{\mathbf{p}}_{f_j}\right]}_{\times }\end{array}$$
这一部分代码对应为：

  // 3d feature position in the world frame.
  // And its observation with the stereo cameras.
  const Vector3d& p_w = feature.position;
  const Vector4d& z = feature.observations.find(cam_state_id)->second;

  // Convert the feature position from the world frame to
  // the cam0 and cam1 frame.
  Vector3d p_c0 = R_w_c0 * (p_w-t_c0_w);
  Vector3d p_c1 = R_w_c1 * (p_w-t_c1_w);

  // Compute the Jacobians.
  Matrix<double, 4, 3> dz_dpc0 = Matrix<double, 4, 3>::Zero();
  dz_dpc0(0, 0) = 1 / p_c0(2);
  dz_dpc0(1, 1) = 1 / p_c0(2);
  dz_dpc0(0, 2) = -p_c0(0) / (p_c0(2)*p_c0(2));
  dz_dpc0(1, 2) = -p_c0(1) / (p_c0(2)*p_c0(2));

  Matrix<double, 4, 3> dz_dpc1 = Matrix<double, 4, 3>::Zero();
  dz_dpc1(2, 0) = 1 / p_c1(2);
  dz_dpc1(3, 1) = 1 / p_c1(2);
  dz_dpc1(2, 2) = -p_c1(0) / (p_c1(2)*p_c1(2));
  dz_dpc1(3, 2) = -p_c1(1) / (p_c1(2)*p_c1(2));

  Matrix<double, 3, 6> dpc0_dxc = Matrix<double, 3, 6>::Zero();
  dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);
  dpc0_dxc.rightCols(3) = -R_w_c0;

  Matrix<double, 3, 6> dpc1_dxc = Matrix<double, 3, 6>::Zero();
  dpc1_dxc.leftCols(3) = R_c0_c1 * skewSymmetric(p_c0);
  dpc1_dxc.rightCols(3) = -R_w_c1;

  Matrix3d dpc0_dpg = R_w_c0;
  Matrix3d dpc1_dpg = R_w_c1;

  H_x = dz_dpc0*dpc0_dxc + dz_dpc1*dpc1_dxc;
  H_f = dz_dpc0*dpc0_dpg + dz_dpc1*dpc1_dpg;

残差的计算为：

  // Compute the residual.
  r = z - Vector4d(p_c0(0)/p_c0(2), p_c0(1)/p_c0(2),
      p_c1(0)/p_c1(2), p_c1(1)/p_c1(2));

上面求得的是第$j$个特征点对第$i$个相机的残差对特征点${\mathbf{H}}_{c_i}^{f_j}$以及相机状态的雅克比${\mathbf{H}}_{c_i}^j$，然后我们将上述雅克比和残差堆叠成第$j$个特征点相关的雅克比矩阵${\mathbf{H}}^{f_j}$和${\mathbf{H}}^j$：

  // Project the residual and Jacobians onto the nullspace
  // of H_fj.  
  H_xj.block<4, 6>(stack_cntr, 21+6*cam_state_cntr) = H_xi;
  H_fj.block<4, 3>(stack_cntr, 0) = H_fi;
  r_j.segment<4>(stack_cntr) = r_i;
  stack_cntr += 4;

从代码看，这里的堆叠顺序是纵向堆叠，紧接着对${\mathbf{H}}^{f_j}$进行QR分解求得其左零空间，QR分解的原理如下$$\begin{array}{l}\mathbf{H}=\mathbf{Q}\mathbf{R}=\left[\begin{array}{cc}{\mathbf{Q}}_1,& {\mathbf{Q}}_2\end{array}\right]\left[\begin{array}{c}{\mathbf{R}}_1\\ 0\end{array}\right]\\ \Rightarrow {\mathbf{Q}}^T\mathbf{H}=\left[\begin{array}{l}{\mathbf{Q}}_1^T\mathbf{H}\\ {\mathbf{Q}}_2^T\mathbf{H}\end{array}\right]=\left[\begin{array}{c}{\mathbf{R}}_1\\ 0\end{array}\right]\\ \Rightarrow {\mathbf{Q}}_2^T\mathbf{H}=0\Rightarrow {\mathbf{V}}^T={\mathbf{Q}}_2^T\end{array}$$其中$\mathbf{H}$的尺寸为$m\times n$，那么${\mathbf{Q}}_1$的尺寸为$m\times n$，${\mathbf{Q}}_2$的尺寸为$m\times (m-n)$，假设观测到特征点$j$的相机数量为$M$，那么${\mathbf{H}}^{f_j}$的尺寸就为$4M\times 3$，那么${\mathbf{V}}^T$的尺寸就为$4M\times (4M-3)$，那么投影到${\mathbf{H}}^{f_j}$左零空间的${\mathbf{H}}_o^j$矩阵尺寸为$(4M-3)\times (21+6C)$其中$C$是状态向量中相机的数量，代码对应为：

  // Project the residual and Jacobians onto the nullspace
  // of H_fj.
  JacobiSVD<MatrixXd> svd_helper(H_fj, ComputeFullU | ComputeThinV);
  MatrixXd A = svd_helper.matrixU().rightCols(
      jacobian_row_size - 3);

  H_x = A.transpose() * H_xj;
  r = A.transpose() * r_j;

在求得每一个特征点相关的雅克比矩阵${\mathbf{H}}_o^j$和残差向量${\mathbf{r}}_j$后同样是纵向堆叠成最终的观测矩阵$\mathbf{H}o$和残差$\mathbf{r}$，代码如下：

  H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
  r.segment(stack_cntr, r_j.rows()) = r_j;
  stack_cntr += H_xj.rows();

可以看出来，这样堆叠出来的观测矩阵如果特征点多的话将是一个可长可长的矩阵，因此在进行观测更新之前我们通过QR分解对观测矩阵进一步压缩：$${\mathbf{H}}_o=\left[\begin{array}{cc}{\mathbf{Q}}_1& {\mathbf{Q}}_2\end{array}\right]\left[\begin{array}{c}{\mathbf{R}}_H\\ 0\end{array}\right]\Rightarrow {\mathbf{r}}_o=\left[\begin{array}{cc}{\mathbf{Q}}_1& {\mathbf{Q}}_2\end{array}\right]\left[\begin{array}{c}{\mathbf{R}}_H\\ 0\end{array}\right]\mathbf{x}+{\mathbf{n}}_o\Rightarrow \left[\begin{array}{c}{\mathbf{Q}}_1^T{\mathbf{r}}_o\\ {\mathbf{Q}}_2^T{\mathbf{r}}_o\end{array}\right]=\left[\begin{array}{c}{\mathbf{T}}_H\\ 0\end{array}\right]\mathbf{x}+\left[\begin{array}{c}{\mathbf{Q}}_1^T{\mathbf{n}}_o\\ {\mathbf{Q}}_2^T{\mathbf{n}}_o\end{array}\right]$$$$\Rightarrow {\mathbf{r}}_n={\mathbf{Q}}_1^T{\mathbf{r}}_o={\mathbf{T}}_H\mathbf{x}+{\mathbf{Q}}_1^T{\mathbf{n}}_o={\mathbf{R}}_H\mathbf{x}+{\mathbf{n}}_n$$对应代码为：

  // Convert H to a sparse matrix.
  SparseMatrix<double> H_sparse = H.sparseView();

  // Perform QR decompostion on H_sparse.
  SPQR<SparseMatrix<double> > spqr_helper;
  spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL);
  spqr_helper.compute(H_sparse);

  MatrixXd H_temp;
  VectorXd r_temp;
  (spqr_helper.matrixQ().transpose() * H).evalTo(H_temp);
  (spqr_helper.matrixQ().transpose() * r).evalTo(r_temp);

  H_thin = H_temp.topRows(21+state_server.cam_states.size()*6);
  r_thin = r_temp.head(21+state_server.cam_states.size()*6);

接下来就是状态正常的卡尔曼更新了，代码如下所示：

  // Compute the Kalman gain.
  const MatrixXd& P = state_server.state_cov;
  MatrixXd S = H_thin*P*H_thin.transpose() +
      Feature::observation_noise