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

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

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

1. 状态预测

首先我们要清楚的一点是，在MSCKF中维护的状态变量是由两部分组成，分别是IMU误差状态xI=[δθi2wT,δbgT,δwvT,δbaT,δwpiT,δθi2cT,δti2cT]T\mathbf{x}_{I}=\left[\delta \boldsymbol{\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 \boldsymbol{\theta}_{\mathrm{i} 2 \mathrm{c}}^{T}, \delta \mathbf{t}_{\mathrm{i} 2 \mathrm{c}}^{T}\right]^{T}xI=[δθi2wT,δbgT,δwvT,δbaT,δwpiT,δθi2cT,δti2cT]T和相机误差状态xc=[δθw2cT,δwpcT]T\mathbf{x}_{c_{}}=\left[\delta \boldsymbol{\theta}_{w 2 c_{}}^{T}, \delta^{\mathrm{w}} \mathbf{p}_{c_{}}^{T}\right]^{T}xc=[δθw2cT,δwpcT]T，在状态预测过程中，只是对IMU的标称状态和IMU的误差状态进行预测，预测的过程中会改变IMU误差状态的协方差以及IMU误差状态相对相机误差状态的协方差。而相机相关的状态只会在观测更新时改变。
在学习MSCKF笔记——误差状态卡尔曼滤波中，我们讨论了IMU的标称状态和误差状态分别的定义和推导，下面我们就可以具体看看，在MSCKF中是如何实际应用这两种状态变量的，对于标称状态，与上一篇博文中不同的是，MSCKF中IMU的标称状态采用的是四阶龙格库塔积分，而不是欧拉积分，精度会更高，四阶龙格库塔不展开进行讨论，我们着重讨论下IMU误差状态的实际应用。
在上一篇博客我们对IMU的误差状态的连续时间系统进行了推导，并进行了近似离散化（精确离散化方法的一阶泰勒展开），这里我们首先给出IMU误差状态连续时间系统方程（这里省略下标III）x˙(t)=F(t)x(t)+G(t)w(t)\dot{\mathbf{x}}(t)=\mathbf{F}(t) \mathbf{x}(t)+\mathbf{G}(t) \mathbf{w}(t)x˙(t)=F(t)x(t)+G(t)w(t)其中状态变量为x=[δθi2wT,δbgT,δwvT,δbaT,δwpiT,δθi2cT,δti2cT]T\mathbf{x}=\left[\delta \boldsymbol{\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 \boldsymbol{\theta}_{\mathrm{i} 2 \mathrm{c}}^{T}, \delta \mathbf{t}_{\mathrm{i} 2 \mathrm{c}}^{T}\right]^{T}x=[δθi2wT,δbgT,δwvT,δbaT,δwpiT,δθi2cT,δti2cT]T，噪声项为w=[ωnT,ωωT,anT,aωT]T\mathbf{w}=\left[\begin{array}{lll}\boldsymbol{\omega}_{n}^{T}, \boldsymbol{\omega}_{\omega}^{T}, \mathbf{a}_{n}^{T}, \mathbf{a}_{\omega}^{T}\end{array}\right]^{T}w=[ωnT,ωωT,anT,aωT]T其中噪声项的均值和方差分别为E[n(t)]=0,E[n(t)Tn(τ)]=qδ(t−τ)E[\mathbf{n}(t)]=0, E\left[\mathbf{n}(t)^{T} \mathbf{n}(\tau)\right]=\mathbf{q} \delta(t-\tau)E[n(t)]=0,E[n(t)Tn(τ)]=qδ(t−τ)F(21×21)=[−[ωm−ωb]×−I000000000000−Ri2w[am−ab]×00−Ri2w000000000000I000000000000000000]\underset{(21 \times 21)}{\mathbf{F}}=\left[\begin{array}{ccccccc} -\left[\omega_{m}-\omega_{b}\right]_{\times} & -\mathbf{I} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ -\mathbf{R}_{\mathrm{i} 2 \mathrm{w}}\left[\mathbf{a}_{m}-\mathbf{a}_{b}\right]_{\times} & \mathbf{0} & \mathbf{0} & -\mathbf{R}_{\mathrm{i} 2 \mathrm{w}} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{I} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \end{array}\right](21×21)F=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡−[ωm−ωb]×0−Ri2w[am−ab]×0000−I0000000000I0000−Ri2w0000000000000000000000000⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤G(21×12)=[−I0000I0000−Ri2w0000I000000000000]\underset{(21 \times 12)}{\mathbf{G}}=\left[\begin{array}{cccc} -\mathbf{I} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{I} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & -\mathbf{R}_{\mathrm{i} 2 \mathrm{w}} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{I} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \end{array}\right](21×12)G=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡−I0000000I0000000−Ri2w0000000I000⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤由于上一篇博客中对IMU的误差状态的连续时间系统进行了推导，唯一不同的是在MSCKF的误差状态中考虑相机和IMU之间的旋转δθi2cT\delta \boldsymbol{\theta}_{\mathrm{i} 2 \mathrm{c}}^{T}δθi2cT和平移δti2cT\delta \mathbf{t}_{\mathrm{i} 2 \mathrm{c}}^{T}δti2cT，对应的代码部分如下：

  // Compute discrete transition and noise covariance matrixMatrix<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();

获得误差状态的连续时间系统推导公式后，我们对其进行离散化，根据线性系统理论，连续时间系统离散化形式为：x(tk+1)=Φ(tk+1,tk)x(tk)+∫tktt+1Φ(tk+1,τ)G(τ)w(τ)dτ\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 \taux(tk+1)=Φ(tk+1,tk)x(tk)+∫tktt+1Φ(tk+1,τ)G(τ)w(τ)dτ我们将短间隔时间内的F(t)\mathbf{F}(t)F(t)矩阵作为常阵Fk\mathbf{F}_{k}Fk处理并对精确离散化公式进行三次泰勒展开，获得Φ(tk+1,tk)=eFΔt=I+FΔt+12(FΔt)2+13!(FΔt)3+O(∥FΔt∥4)\boldsymbol{\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(\|\mathbf{F} \Delta t\|^{4}\right)Φ(tk+1,tk)=eFΔt=I+FΔt+21(FΔt)2+3!1(FΔt)3+O(∥FΔt∥4)同时，我们令Wk=∫tktt+1Φ(tk+1,τ)G(τ)w(τ)dτ\mathbf{W}_{k}=\int_{t_{k}}^{t_{t+1}} \boldsymbol{\Phi}\left(t_{k+1}, \tau\right) \mathbf{G}(\tau) \mathbf{w}(\tau) d \tauWk=∫tktt+1Φ(tk+1,τ)G(τ)w(τ)dτ则离散化后即获得运动方程为xk+1=Φk+1∣kxk+Wk\mathbf{x}_{k+1}=\boldsymbol{\Phi}_{k+1 \mid k} \mathbf{x}_{k}+\mathbf{W}_{k}xk+1=Φk+1∣kxk+Wk我们计算噪声项Wk\mathbf{W}_{k}Wk的均值和方差分别为E[Wk]=0,E[WkWkT]=Qkδ(t−τ)E\left[\mathbf{W}_{k}\right]=0, E\left[\mathbf{W}_{k} \mathbf{W}_{k}^{T}\right]=\mathbf{Q}_{k} \delta(t-\tau)E[Wk]=0,E[WkWkT]=Qkδ(t−τ)，其中Qk=∫tktk+1Φ(tk+1,t)G(t)qG(t)TΦT(tk+1,t)dt\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) d tQk=∫tktk+1Φ(tk+1,t)G(t)qG(t)TΦT(tk+1,t)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;

我们求得运动方程的的噪声项方差的主要目的是进行卡尔曼滤波的运动学推导，我们可以不管误差状态的均值，因为误差状态是噪声驱动的，噪声项的均值为零，在运动方程的迭代下均值始终为零，而误差状态的方差则会收到噪声方差影响：PIIk+1∣k=Φk+1∣kPIIk∣kΦk+1∣kT+Qk\mathbf{P}_{I I_{k+1 \mid k}}=\boldsymbol{\Phi}_{k+1 \mid k} \mathbf{P}_{I I_{k\mid k}}\boldsymbol{\Phi}_{k+1 \mid k}^{T}+\mathbf{Q}_{k}PIIk+1∣k=Φk+1∣kPIIk∣kΦk+1∣kT+Qk同时，如果有相机状态存在时，由于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;

对于误差状态，均值没啥好扩增的，因为在观测发生前，所有误差状态的均值都为零，我们最为关心的应该是误差状态的协方差矩阵扩增，这里借用吴博在深蓝学院课程上将的概念，矩阵扩增分为理解为克隆和修正两步，第一步克隆如下：[xi]⟶[xixi]\left[\mathbf{x}_{i}\right] \longrightarrow\left[\begin{array}{l} \mathbf{x}_{i} \\ \mathbf{x}_{i} \end{array}\right][xi]⟶[xixi][Pi]⟶[PiiPiiPiiPii]\left[\mathbf{P}_{i}\right] \longrightarrow\left[\begin{array}{cc} \mathbf{P}_{i i} & \mathbf{P}_{i i} \\ \mathbf{P}_{i i} & \mathbf{P}_{i i} \end{array}\right][Pi]⟶[PiiPiiPiiPii]第二步修正如下：[xi]⟶[xixj]\left[\mathbf{x}_{i}\right] \longrightarrow\left[\begin{array}{l} \mathbf{x}_{i} \\ \mathbf{x}_{j} \end{array}\right][xi]⟶[xixj]xj=Jjixi\mathbf{x}_{j}=\mathbf{J}_{j i} \mathbf{x}_{i}xj=Jjixi[PiiPiiPiiPii]⟶[PiiPjiPijPjj]\left[\begin{array}{ll} \mathbf{P}_{i i} & \mathbf{P}_{i i} \\ \mathbf{P}_{i i} & \mathbf{P}_{i i} \end{array}\right] \longrightarrow\left[\begin{array}{ll} \mathbf{P}_{i i} & \mathbf{P}_{j i} \\ \mathbf{P}_{i j} & \mathbf{P}_{j j} \end{array}\right][PiiPiiPiiPii]⟶[PiiPijPjiPjj]Pij=JjiPii\mathbf{P}_{i j}=\mathbf{J}_{j i}\mathbf{P}_{i i}Pij=JjiPiiPjj=JjiPiiJjiT\mathbf{P}_{j j}=\mathbf{J}_{j i} \mathbf{P}_{i i} \mathbf{J}_{j i}^{T}Pjj=JjiPiiJjiT其中Jji\mathbf{J}_{j i}Jji为新增状态变量xj\mathbf{x}_{j}xj相对xi\mathbf{x}_{i}xi的雅克比矩阵，那么在MSCKF中，新增误差相机状态对当前误差状态的雅克比为J(6,21+6N)=[(∂xc∂xIT)T,(∂xc∂xcT)T]T\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}J(6,21+6N)=[(∂xIT∂xc)T,(∂xcT∂xc)T]T其中第二项是当前相机误差状态变量相对其他误差相机状态变量的的雅克比，为零矩阵，第一项为新增相机误差状态变量相对IMU误差状态变量的雅克比，结果如下（这一部分是参考吴博深蓝课程上的推导，我感觉推导过程中还有些问题和代码对应也不是完全一致，这里后续修改）：∂xc∂xIT=[∂δθc2w∂δθi2w03×903×3∂δθc2w∂δθi2c03×3∂δwpc∂δθi2w03×9∂δwpcδwpi03×3∂δwpcδti2c]\frac{\partial \mathbf{x}_{c}}{\partial \mathbf{x}_{I}^{T}}=\left[\begin{array}{ccccc} \frac{\partial\delta \boldsymbol{\theta}_{c 2 w_{}}}{\partial\delta \boldsymbol{\theta}_{\mathrm{i} 2 \mathrm{w}}} & \mathbf{0}_{3\times9} & \mathbf{0}_{3\times3} & \frac{\partial\delta \boldsymbol{\theta}_{c 2 w_{}}}{\partial\delta \boldsymbol{\theta}_{\mathrm{i} 2 \mathrm{c}}} & \mathbf{0}_{3\times3} \\ \frac{\partial \delta^{\mathrm{w}} \mathbf{p}_{c_{}}}{\partial\delta \boldsymbol{\theta}_{\mathrm{i} 2 \mathrm{w}}} & \mathbf{0}_{3\times9} &\frac {\partial\delta^{\mathrm{w}} \mathbf{p}_{c_{}}}{\delta^{\mathrm{w}} \mathbf{p}_{i_{}}} & \mathbf{0}_{3\times3} & \frac{\partial\delta^{\mathrm{w}} \mathbf{p}_{c_{}}}{\delta \mathbf{t}_{\mathrm{i} 2 \mathrm{c}}} \end{array}\right]∂xIT∂xc=[∂δθi2w∂δθc2w∂δθi2w∂δwpc03×903×903×3δwpi∂δwpc∂δθi2c∂δθc2w03×303×3δti2c∂δwpc]
我们已知：{wpc=Ri2wipc+wpiRw2c=Ri2cRw2i\left\{\begin{array}{l} {}^w \mathbf{p}_{c}=\mathbf{R}_{i 2 w}{}^{i} \mathbf{p}_{c}+{ }^{w} \mathbf{p}_{i} \\ \mathbf{R}_{w 2 c}=\mathbf{R}_{i 2 c} \mathbf{R}_{w 2 i} \end{array}\right.{wpc=Ri2wipc+wpiRw2c=Ri2cRw2i
对于∂δθc2w∂δθi2w\frac{\partial\delta \boldsymbol{\theta}_{c 2 w_{}}}{\partial\delta \boldsymbol{\theta}_{\mathrm{i} 2 \mathrm{w}}}∂δθi2w∂δθc2w:Rc2wT=Ri2cRi2wT⇒(Rc2wExp⁡(δθc2w))T=Ri2c(Ri2wExp⁡(δθi2w))T⇒Exp⁡(−δθc2w)Rw2c=Ri2cExp⁡(−δθi2w)Rw2i⇒Exp⁡(−δθc2w)Rw2c=Exp⁡(Adj⁡Ri2c(−δθi2w))Ri2cRw2i⇒Exp⁡(−δθc2w)=Exp⁡(Adj⁡Ri2c(−δθi2w))⇒δθc2w=Ri2cδθi2w\begin{array}{l} \mathbf{R}_{c 2 w}^{T}=\mathbf{R}_{i 2 c} \mathbf{R}_{i 2 w}^{T} \\ \Rightarrow\left(\mathbf{R}_{c 2 w} \operatorname{Exp}\left(\delta \boldsymbol{\theta}_{c2w}\right)\right)^{T}=\mathbf{R}_{i 2 c}\left(\mathbf{R}_{i 2 w} \operatorname{Exp}\left(\delta \boldsymbol{\theta}_{i2w}\right)\right)^{T} \\ \Rightarrow \operatorname{Exp}\left(-\delta \boldsymbol{\theta}_{c 2 w}\right) \mathbf{R}_{w 2 c}=\mathbf{R}_{i 2 c} \operatorname{Exp}\left(-\delta \boldsymbol{\theta}_{i 2 w}\right) \mathbf{R}_{w 2 i} \\ \Rightarrow \operatorname{Exp}\left(-\delta \boldsymbol{\theta}_{c 2 w}\right) \mathbf{R}_{w 2 c}=\operatorname{Exp}\left(\operatorname{Adj}_{\mathbf{R}_{i2c}}\left(-\delta \boldsymbol{\theta}_{i 2 w}\right)\right) \mathbf{R}_{i 2 c} \mathbf{R}_{w2i} \\ \Rightarrow \operatorname{Exp}\left(-\delta \boldsymbol{\theta}_{c 2 w}\right)=\operatorname{Exp}\left(\operatorname{Adj}_{\mathbf{R}_{i2c}}\left(-\delta \boldsymbol{\theta}_{i 2 w}\right)\right) \\ \Rightarrow \delta \boldsymbol{\theta}_{c 2 w}=\mathbf{R}_{i 2 c} \delta \boldsymbol{\theta}_{i 2 w} \end{array}Rc2wT=Ri2cRi2wT⇒(Rc2wExp(δθc2w))T=Ri2c(Ri2wExp(δθi2w))T⇒Exp(−δθc2w)Rw2c=Ri2cExp(−δθi2w)Rw2i⇒Exp(−δθc2w)Rw2c=Exp(AdjRi2c(−δθi2w))Ri2cRw2i⇒Exp(−δθc2w)=Exp(AdjRi2c(−δθi2w))⇒δθc2w=Ri2cδθi2w∂δθc2w∂δθi2w=Ri2c\frac{\partial\delta \boldsymbol{\theta}_{c 2 w_{}}}{\partial\delta \boldsymbol{\theta}_{\mathrm{i} 2 \mathrm{w}}}=\mathbf{R}_{i 2 c}∂δθi2w∂δθc2w=Ri2c对于∂δθc2w∂δθi2c\frac{\partial\delta \boldsymbol{\theta}_{c 2 w_{}}}{\partial\delta \boldsymbol{\theta}_{\mathrm{i} 2 \mathrm{c}}}∂δθi2c∂δθc2w：Rw2c=Ri2cRw2i⇒(Rc2wExp⁡(δθc2w))T=(Rc2iExp⁡(δθc2i))TRw2i⇒Exp⁡(−δθc2w)Rw2c=Exp⁡(−δθc2i)Ri2cRw2i⇒δθc2w=δθc2i\begin{array}{l} \mathbf{R}_{w 2 c}=\mathbf{R}_{i 2 c} \mathbf{R}_{w 2 i} \\ \Rightarrow\left(\mathbf{R}_{c 2 w} \operatorname{Exp}\left(\delta\boldsymbol{\theta}_{c 2 w}\right)\right)^{T}=\left(\mathbf{R}_{c 2 i} \operatorname{Exp}\left(\delta\boldsymbol{\theta}_{c 2 i}\right)\right)^{T} \mathbf{R}_{w 2 i} \\ \Rightarrow \operatorname{Exp}\left(-\delta\boldsymbol{\theta}_{c 2 w}\right) \mathbf{R}_{w 2 c}=\operatorname{Exp}\left(-\delta\boldsymbol{\theta}_{c 2 i}\right) \mathbf{R}_{i 2 c} \mathbf{R}_{w 2 i} \\ \Rightarrow \delta\boldsymbol{\theta}_{c 2 w}=\delta\boldsymbol{\theta}_{c 2 i} \end{array}Rw2c=Ri2cRw2i⇒(Rc2wExp(δθc2w))T=(Rc2iExp(δθc2i))TRw2i⇒Exp(−δθc2w)Rw2c=Exp(−δθc2i)Ri2cRw2i⇒δθc2w=δθc2i∂δθc2w∂δθi2c=I\frac{\partial\delta \boldsymbol{\theta}_{c 2 w_{}}}{\partial\delta \boldsymbol{\theta}_{\mathrm{i} 2 \mathrm{c}}}=\mathbf{I}∂δθi2c∂δθc2w=I对于∂δwpc∂δθi2w\frac{\partial \delta^{\mathrm{w}} \mathbf{p}_{c_{}}}{\partial\delta \boldsymbol{\theta}_{\mathrm{i} 2 \mathrm{w}}}∂δθi2w∂δwpc:∂wpc∂δθi2w=lim⁡δθ→0(Ri2wExp⁡(δθ)ipc+wpi)−(Ri2wipc+wpi)δθ=lim⁡δθ→0(Ri2w(I+[δθ]×)ipc+wpi)−(Ri2wpc+wpi)δθ=lim⁡δθ→0Ri2w[δθ]×ipcδθ=−Ri2w[ipc]×\begin{aligned} \frac{\partial^{w} \mathbf{p}_{c}}{\partial \delta \boldsymbol{\theta}_{i 2 w}} &=\lim _{\delta \boldsymbol{\theta} \rightarrow 0} \frac{\left(\mathbf{R}_{i 2 w} \operatorname{Exp}(\delta \boldsymbol{\theta})^{i} \mathbf{p}_{c}+{ }^{w} \mathbf{p}_{i}\right)-\left(\mathbf{R}_{i 2 w}^{i} \mathbf{p}_{c}+{ }^{w} \mathbf{p}_{i}\right)}{\delta \boldsymbol{\theta}}\\ &=\lim _{\delta \theta \rightarrow 0} \frac{\left(\mathbf{R}_{i 2 w}\left(\mathbf{I}+[\delta \boldsymbol{\theta}]_{\times}\right)^{i} \mathbf{p}_{c}+{ }^{w} \mathbf{p}_{i}\right)-\left(\mathbf{R}_{i 2 w}{\mathbf{p}}_{c}+{ }^{w} \mathbf{p}_{i}\right)}{\delta \boldsymbol{\theta}}\\ &=\lim _{\delta \boldsymbol{\theta} \rightarrow 0} \frac{\mathbf{R}_{i 2 w}[\delta \boldsymbol{\theta}]_{\times}^{i} \mathbf{p}_{c}}{\delta \boldsymbol{\theta}}=-\mathbf{R}_{i 2 w}\left[{ }^{i} \mathbf{p}_{c}\right]_{\times} \end{aligned}∂δθi2w∂wpc=δθ→0limδθ(Ri2wExp(δθ)ipc+wpi)−(Ri2wipc+wpi)=δθ→0limδθ(Ri2w(I+[δθ]×)ipc+wpi)−(Ri2wpc+wpi)=δθ→0limδθRi2w[δθ]×ipc=−Ri2w[ipc]×对于∂δwpcδwpi\frac {\partial\delta^{{w}} \mathbf{p}_{c_{}}}{\delta^{{w}} \mathbf{p}_{i_{}}}δwpi∂δwpc:∂δwpcδwpi=lim⁡δ′pϵ→0(Ri2w(ipc+δipc)+wpi)−(Ripc+wpi)δθ=lim⁡δθ→0Ri2wδipcδipc=Ri2w\begin{aligned}\frac {\partial\delta^{{w}} \mathbf{p}_{c_{}}}{\delta^{{w}} \mathbf{p}_{i_{}}}&=\lim _{\delta^{\prime} \mathbf{p}_{\epsilon} \rightarrow 0} \frac{\left(\mathbf{R}_{i 2 w}\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 \mathbf{\theta}}\\&=\lim _{\delta \boldsymbol{\theta} \rightarrow 0} \frac{\mathbf{R}_{i 2 w} \delta^{i} \mathbf{p}_{c}}{\delta^{i} \mathbf{p}_{c}}=\mathbf{R}_{i 2 w}\end{aligned}δwpi∂δwpc=δ′pϵ→0limδθ(Ri2w(ipc+δipc)+wpi)−(Ripc+wpi)=δθ→0limδipcRi2wδipc=Ri2w对于∂δwpcδti2c\frac{\partial\delta^{\mathrm{w}} \mathbf{p}_{c_{}}}{\delta \mathbf{t}_{\mathrm{i} 2 \mathrm{c}}}δti2c∂δwpc:∂wpc∂δwpi=I\frac{\partial^{w} \mathbf{p}_{c}}{\partial \delta^{w} \mathbf{p}_{i}}=\mathbf{I}∂δwpi∂wpc=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. 状态更新

状态扩增后就开始构建观测并对误差状态进行更新，也就是执行卡尔曼滤波中的后三步，状态更新的前提以及三角化等操作这里就不赘述了，直接看前面的流程图就很容易搞清楚，这其中重要的步骤就是状态观测如何构建，构建的过程其实也比较简单，这一部分推导主要参考吴博在深蓝学院课程上的讲解，通过重投影误差构建观测残差：rj=zj−z^j\mathbf{r}^{j}=\mathbf{z}^{j}-\widehat{\mathbf{z}}^{j}rj=zj−zj其中jjj代表第jjj个特征点，然后将残差对状态变量进行线性化，也就是求残差对状态变量的雅克比，因为观测残差和特征点还有关系，因此我们还要求残差对特征点的雅克比：rj≃Hjx+Hfjwpj+nj\mathbf{r}^{j} \simeq \mathbf{H}^{j} {\mathbf{x}}+\mathbf{H}^{f_{j} w} {\mathbf{p}}_{j}+\mathbf{n}^{j}rj≃Hjx+Hfjwpj+nj但是由于我们的状态变量中没有特征点的部分，因此最后我们还需要将残差rj\mathbf{r}^{j}rj和状态变量雅克比Hj\mathbf{H}^{j}Hj投影到特征点雅克比的零空间中去，其实也就是消元掉观测方程中特征点相关的部分：roj=VT(zj−z^j)≃VTHjx+VTHfjpj+VTnj=VTHjx+VTnj=Hojx+noj\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}roj=VT(zj−z^j)≃VTHjx+VTHfjpj+VTnj=VTHjx+VTnj=Hojx+noj关于这中间的细节，我们首先看下雅克比是怎么求的。首先我们考虑世界坐标系下的第jjj个3D点wpfj=[x,y,z]T{ }^{w} \mathbf{p}_{f_{j}}=[x, y, z]^{T}wpfj=[x,y,z]T，该3D点被世界坐标系下的第iii个双目wpci1,wpci2{ }^{w} \mathbf{p}_{c_{i 1}},{ }^{w} \mathbf{p}_{c_{i 2}}wpci1,wpci2观测到，对应到归一化平面上的坐标为zij=[u,v]T\mathbf{z}_{i }^j=[u, v]^{T}zij=[u,v]T，那么zij=[uv]=[xzyz]\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]zij=[uv]=[zxzy]第jjj个特征点与第iii个相机构成的残差为：rij=zij−z^ij\mathbf{r}_{i}^{j}=\mathbf{z}_{i}^{j}-\widehat{\mathbf{z}}_{i}^{j}rij=zij−zij因为我们在MSCKF中使用的是双目，因此会有ri1j\mathbf{r}_{i1}^{j}ri1j和ri2j\mathbf{r}_{i2}^{j}ri2j两个残差，而在双目状态中我们以左目状态为准，那么残差对特征点的雅克比为Hcifj4×3=[∂ri1j∂wpci1∂ri2j∂wpci1]=[∂ri1j∂ci1pfj∂ci1pfj∂wpfj∂ri2j∂ci2pfj∂ci2pfj∂wpfj]\begin{array}{c} \mathbf{H}_{c_i}^{f_{j}} \\ 4 \times 3 \end{array}=\left[\begin{array}{c} \frac{\partial \mathbf{r}_{i 1}^{j}}{\partial^{w} \mathbf{p}_{c_{i 1}}} \\ \frac{\partial \mathbf{r}_{i 2}^{j}}{\partial^{w} \mathbf{p}_{c_{i 1}}} \end{array}\right]=\left[\begin{array}{c} \frac{\partial \mathbf{r}_{i 1}^{j}}{\partial^{c_{i 1}} \mathbf{p}_{f_{j}}} \frac{\partial^{c_{i 1}} \mathbf{p}_{f_{j}}}{\partial^{w} \mathbf{p}_{f_{j}}} \\ \frac{\partial \mathbf{r}_{i 2}^{j}}{\partial^{c_{i 2}} \mathbf{p}_{f_{j}}} \frac{\partial^{c_{i 2}} \mathbf{p}_{f_{j}}}{\partial^{w} \mathbf{p}_{f_{j}}} \end{array}\right]Hcifj4×3=⎣⎡∂wpci1∂ri1j∂wpci1∂ri2j⎦⎤=⎣⎡∂ci1pfj∂ri1j∂wpfj∂ci1pfj∂ci2pfj∂ri2j∂wpfj∂ci2pfj⎦⎤残差对相机状态的雅克比为：Hcij4×6=[∂ri1j∂Rw2ci1,∂ri1j∂wpci1∂ri2j∂Rw2ci1,∂ri1j∂wpci1]=[∂ri1j∂ci1pfj∂ci1pfj∂Rw2ci1,∂ri1j∂ci1pfj∂ci1pfj∂wpci1∂ri2j∂ci2pfj∂ci2pfj∂Rw2ci1,∂ri2j∂ci2pfj∂ci2pfj∂wpci1]\begin{array}{c} \mathbf{H}_{c_{i}}^{j} \\ 4 \times 6 \end{array}=\left[\begin{array}{cc} \frac{\partial \mathbf{r}_{i 1}^{j}}{\partial \mathbf{R}_{w 2 c_{i 1}}}, & \frac{\partial \mathbf{r}_{i 1}^{j}}{\partial^{w} \mathbf{p}_{c_{i 1}}} \\ \frac{\partial \mathbf{r}_{i 2}^{j}}{\partial \mathbf{R}_{w 2 c_{i 1}}}, & \frac{\partial \mathbf{r}_{i 1}^{j}}{\partial^{w} \mathbf{p}_{c_{i 1}}} \end{array}\right]=\left[\begin{array}{c} \frac{\partial \mathbf{r}_{i 1}^{j}}{\partial^{c_{i 1}} \mathbf{p}_{f_{j}}} \frac{\partial^{c_{i 1}} \mathbf{p}_{f_{j}}}{\partial \mathbf{R}_{w 2 c_{i 1}}}, \frac{\partial \mathbf{r}_{i 1}^{j}}{\partial^{c_{i 1}} \mathbf{p}_{f_{j}}} \frac{\partial^{c_{i 1}} \mathbf{p}_{f_{j}}}{\partial^{w} \mathbf{p}_{c_{i 1}}} \\ \frac{\partial \mathbf{r}_{i 2}^{j}}{\partial^{c_{i 2}} \mathbf{p}_{f_{j}}} \frac{\partial^{c_{i 2}} \mathbf{p}_{f_{j}}}{\partial \mathbf{R}_{w 2 c_{i 1}}}, \frac{\partial \mathbf{r}_{i 2}^{j}}{\partial^{c_{i 2}} \mathbf{p}_{f_{j}}} \frac{\partial^{c_{i 2}} \mathbf{p}_{f_{j}}}{\partial^{w} \mathbf{p}_{c_{i 1}}} \end{array}\right]Hcij4×6=⎣⎡∂Rw2ci1∂ri1j,∂Rw2ci1∂ri2j,∂wpci1∂ri1j∂wpci1∂ri1j⎦⎤=⎣⎡∂ci1pfj∂ri1j∂Rw2ci1∂ci1pfj,∂ci1pfj∂ri1j∂wpci1∂ci1pfj∂ci2pfj∂ri2j∂Rw2ci1∂ci2pfj,∂ci2pfj∂ri2j∂wpci1∂ci2pfj⎦⎤我们知道双目的投影方程{left⁡:ci1pfj=Rw2ci1(wpfj−wpci1)right: ci2pfj=Rci12ci2(Rw2ci1(wpfj−wpci1))+tci12ci2\left\{\begin{array}{l} \operatorname{left}:^{c_{i1}} \mathbf{p}_{f_{j}}=\mathbf{R}_{w 2 c_{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} 2 c_{i2}}\left(\mathbf{R}_{w 2 c_{i1}}\left({}^{w}{\mathbf{p}}_{f_{j}}-{ }^{w} \mathbf{p}_{c_{i1}}\right)\right)+\mathbf{t}_{c_{i 1} 2 c_{i 2}} \end{array}\right.{left:ci1pfj=Rw2ci1(wpfj−wpci1) right: ci2pfj=Rci12ci2(Rw2ci1(wpfj−wpci1))+tci12ci2然后根据这个投影方程求解雅克比∂rij∂cpfj=[∂u∂x=1z,∂u∂y=0,∂u∂z=−xz2∂u∂x=0,∂u∂y=1z,∂u∂z=−yz2]\frac{\partial \mathbf{r}_{i}^{j}}{\partial^{c} \mathbf{p}_{f_{j}}}=\left[\begin{array}{ll} \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]∂cpfj∂rij=[∂x∂u=z1,∂x∂u=0,∂y∂u=0,∂y∂u=z1,∂z∂u=−z2x∂z∂u=−z2y]∂ci1pfj∂wpfj=Rw2ci1∂ci2pfj∂wpfj=Rw2ci2\frac{\partial^{c_{i1}} \mathbf{p}_{f_{j}}}{\partial^{w} \mathbf{p}_{f_{j}}}=\mathbf{R}_{w 2 c_{i1}} \quad \frac{\partial^{c_{i2}} \mathbf{p}_{f_{j}}}{\partial^{w} \mathbf{p}_{f_{j}}}=\mathbf{R}_{w 2 c_{i2}}∂wpfj∂ci1pfj=Rw2ci1∂wpfj∂ci2pfj=Rw2ci2∂ci1pfj∂wpci1=−Rw2ci1∂ci2pfj∂wpci1=−Rw2ci2\frac{\partial^{c_{i 1}} \mathbf{p}_{f_{j}}}{\partial^{w} \mathbf{p}_{c_{i 1}}}=-\mathbf{R}_{w 2 c_{i 1}} \quad \frac{\partial^{c_{i 2}} \mathbf{p}_{f_{j}}}{\partial^{w} \mathbf{p}_{c_{i 1}}}=-\mathbf{R}_{w 2 c_{i 2}}∂wpci1∂ci1pfj=−Rw2ci1∂wpci1∂ci2pfj=−Rw2ci2∂ci1pfj∂Rw2ci1=lim⁡δθ→0(Rci12wExp⁡(δθ))T(wpfj−wpci1)−Rw2ci1(wpfj−wpci1)δθ=lim⁡δθ→0(I−[δθ]x)Rw2ci1(wpfj−wpci1)−Rw2ci1(wpfj−wpci1)δθ=lim⁡δθ→0−[δθ]×Rw2ci1(wpfj−wpci1)δθ=[Rw2ci1(wpfj−wpci1)]×=[ci1pfj]×\begin{aligned} \frac{\partial^{c_{i1}} \mathbf{p}_{f_{j}}}{\partial \mathbf{R}_{w 2 c_{i1}}}&=\lim _{\delta \boldsymbol{\theta} \rightarrow 0} \frac{\left(\mathbf{R}_{c_{i1} 2 w} \operatorname{Exp}(\delta \boldsymbol{\theta})\right)^{T}\left({}^w \mathbf{p}_{f_{j}}-{ }^{w} \mathbf{p}_{c_{i1}}\right)-\mathbf{R}_{w 2 c_{i1}}\left({}^w \mathbf{p}_{f_{j}}-{}^w \mathbf{p}_{c_{i1}}\right)}{\delta \mathbf{\theta}}\\ &=\lim _{\delta \boldsymbol{\theta} \rightarrow 0} \frac{\left(\mathbf{I}-[\delta \boldsymbol{\theta}]_{x}\right) \mathbf{R}_{w 2 c_{i1}}\left({ }^{w} \mathbf{p}_{f_{j}}-{ }^{w} \mathbf{p}_{c_{i1}}\right)-\mathbf{R}_{w 2 c_{i1}}\left({ }^{w} \mathbf{p}_{f_{j}}-{ }^{w} \mathbf{p}_{c_{i1}}\right)}{\delta \mathbf{\theta}}\\ &=\lim _{\delta \boldsymbol{\theta} \rightarrow 0} \frac{-[\delta \boldsymbol{\theta}]_{\times} \mathbf{R}_{w 2 c_{i1}}\left({ }^{w} \mathbf{p}_{f_{j}}-{{}^w\mathbf{p}}_{c_{i1}}\right)}{\delta \boldsymbol{\theta}}\\ &=\left[\mathbf{R}_{w 2 c_{i 1}}\left({ }^{w} \mathbf{p}_{f_{j}}-{ }^{w} \mathbf{p}_{c_{i1}}\right)\right]_{\times}=\left[{ }^{c_{i1}} \mathbf{p}_{f_{j}}\right]_{\times} \end{aligned}∂Rw2ci1∂ci1pfj=δθ→0limδθ(Rci12wExp(δθ))T(wpfj−wpci1)−Rw2ci1(wpfj−wpci1)=δθ→0limδθ(I−[δθ]x)Rw2ci1(wpfj−wpci1)−Rw2ci1(wpfj−wpci1)=δθ→0limδθ−[δθ]×Rw2ci1(wpfj−wpci1)=[Rw2ci1(wpfj−wpci1)]×=[ci1pfj]×∂ci2pfj∂Rw2ci1=lim⁡δθ→0(Rci12wExp⁡(δθ)Rci22ci1)T(wpfj−wpci1)−Rci12ci2Rw2ci1(wpfj−wpci1)δθ=lim⁡δθ→0Rci12ci2(I−[δθ]×)Rw2ci1(wpfj−wpci1)−Rc112ci2Rw2ci1(wpfj−wpci1)δθ=Rci12ci2[Rw2c1(wpfj−wpci1)]×=Rci12ci2[ci1pfj]×\begin{aligned} \frac{\partial^{c_{i2}} \mathbf{p}_{f_{j}}}{\partial \mathbf{R}_{w 2 c_{i1}}}&=\lim _{\delta \boldsymbol{\theta} \rightarrow 0} \frac{\left(\mathbf{R}_{c_{i1} 2 w} \operatorname{Exp}(\delta \boldsymbol{\theta}) \mathbf{R}_{c_{i2} 2 c_{i1}}\right)^{T}\left({ }^{w} \mathbf{p}_{f_{j}}-{ }^{w} \mathbf{p}_{c_{i1}}\right)-\mathbf{R}_{c_{i1} 2 c_{i2}} \mathbf{R}_{w 2 c_{i1}}\left({}^w \mathbf{p}_{f_{j}}-{}^w{\mathbf{p}_{c_{i1}}}\right)}{\delta \boldsymbol{\theta}}\\ &=\lim _{\delta \boldsymbol{\theta} \rightarrow 0} \frac{\mathbf{R}_{c_{i 1} 2 c_{i 2}}\left(\mathbf{I}-[\delta \boldsymbol{\theta}]_{\times}\right) \mathbf{R}_{w 2 c_{i1}}\left({ }^{w} \mathbf{p}_{f_{j}}-{ }^{w} \mathbf{p}_{c_{i1}}\right)-\mathbf{R}_{c_{11} 2 c_{i 2}} \mathbf{R}_{w 2 c_{i1}}\left(w_{\mathbf{p}_{f_{j}}}-w_{\mathbf{p}_{c_{i1}}}\right)}{\delta \boldsymbol{\theta}}\\ &=\mathbf{R}_{c_{i_{1}} 2 c_{i_{2}}}\left[\mathbf{R}_{{}w 2 c_{1}}\left({}^w{\mathbf{p}_{f_{j}}}-{}^w{\mathbf{p}_{c_{i1}}}\right)\right]_{\times}=\mathbf{R}_{c_{i1} 2 c_{i2}}\left[{}^{c_{i1}} \mathbf{p}_{f_{j}}\right]_{\times} \end{aligned}∂Rw2ci1∂ci2pfj=δθ→0limδθ(Rci12wExp(δθ)Rci22ci1)T(wpfj−wpci1)−Rci12ci2Rw2ci1(wpfj−wpci1)=δθ→0limδθRci12ci2(I−[δθ]×)Rw2ci1(wpfj−wpci1)−Rc112ci2Rw2ci1(wpfj−wpci1)=Rci12ci2[Rw2c1(wpfj−wpci1)]×=Rci12ci2[ci1pfj]×
这一部分代码对应为：

  // 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));

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

  // 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;

从代码看，这里的堆叠顺序是纵向堆叠，紧接着对Hfj\mathbf{H}^{f_j}Hfj进行QR分解求得其左零空间，QR分解的原理如下H=QR=[Q1,Q2][R10]⇒QTH=[Q1THQ2TH]=[R10]⇒Q2TH=0⇒VT=Q2T\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} \\ \mathbf{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} \\ \mathbf{0} \end{array}\right] \\ \Rightarrow \mathbf{Q}_{2}^{T} \mathbf{H}=\mathbf{0} \Rightarrow \mathbf{V}^{T}=\mathbf{Q}_{2}^{T} \end{array}H=QR=[Q1,Q2][R10]⇒QTH=[Q1THQ2TH]=[R10]⇒Q2TH=0⇒VT=Q2T其中H\mathbf{H}H的尺寸为m×nm \times nm×n，那么Q1\mathbf{Q}_1Q1的尺寸为m×nm \times nm×n，Q2\mathbf{Q}_2Q2的尺寸为m×(m−n)m \times (m-n)m×(m−n)，假设观测到特征点jjj的相机数量为MMM，那么Hfj\mathbf{H}^{f_j}Hfj的尺寸就为4M×34M \times 34M×3，那么VT\mathbf{V}^TVT的尺寸就为4M×(4M−3)4M \times (4M-3)4M×(4M−3)，那么投影到Hfj\mathbf{H}^{f_j}Hfj左零空间的Hoj\mathbf{H}^{j}_oHoj矩阵尺寸为(4M−3)×(21+6C)(4M-3) \times (21+6C)(4M−3)×(21+6C)其中CCC是状态向量中相机的数量，代码对应为：

  // 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;

在求得每一个特征点相关的雅克比矩阵Hoj\mathbf{H}^{j}_oHoj和残差向量rj\mathbf{r}_jrj后同样是纵向堆叠成最终的观测矩阵Ho\mathbf{H}oHo和残差r\mathbf{r}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分解对观测矩阵进一步压缩：Ho=[Q1Q2][RH0]⇒ro=[Q1Q2][RH0]x+no⇒[Q1TroQ2Tro]=[TH0]x+[Q1TnoQ2Tno]\mathbf{H}_{o}=\left[\begin{array}{cc} \mathbf{Q}_{1} & \mathbf{Q}_{2} \end{array}\right]\left[\begin{array}{c} \mathbf{R}_{H} \\ \mathbf{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} \\ \mathbf{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} \\ \mathbf{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]Ho=[Q1Q2][RH0]⇒ro=[Q1Q2][RH0]x+no⇒[Q1TroQ2Tro]=[TH0]x+[Q1TnoQ2Tno]⇒rn=Q1Tro=THx+Q1Tno=RHx+nn\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}⇒rn=Q1Tro=THx+Q1Tno=RHx+nn对应代码为：

  // 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*MatrixXd::Identity(H_thin.rows(), H_thin.rows());//MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H_thin*P);MatrixXd K_transpose = S.ldlt().solve(H_thin*P);MatrixXd K = K_transpose.transpose();// Compute the error of the state.VectorXd delta_x = K * r_thin;// Update state covariance.MatrixXd I_KH = MatrixXd::Identity(K.rows(), H_thin.cols()) - K*H_thin;state_server.state_cov = I_KH*state_server.state_cov;

这里看不懂的同学直接找个扩展卡尔曼更新的公式就能对得上了，这里值得注意的两点是，观测的噪声不相关，因此是噪声系数乘以单位阵，因为我们前面提到，误差状态的均值在进行观测更新始终为零，因此更新误差状态均值delta_x时直接等于K * r_thin，然后拿误差状态均值去修正标称状态的各个状态量，代码如下：

  const Vector4d dq_imu =smallAngleQuaternion(delta_x_imu.head<3>());state_server.imu_state.orientation = quaternionMultiplication(dq_imu, state_server.imu_state.orientation);state_server.imu_state.gyro_bias += delta_x_imu.segment<3>(3);state_server.imu_state.velocity += delta_x_imu.segment<3>(6);state_server.imu_state.acc_bias += delta_x_imu.segment<3>(9);state_server.imu_state.position += delta_x_imu.segment<3>(12);const Vector4d dq_extrinsic =smallAngleQuaternion(delta_x_imu.segment<3>(15));state_server.imu_state.R_imu_cam0 = quaternionToRotation(dq_extrinsic) * state_server.imu_state.R_imu_cam0;state_server.imu_state.t_cam0_imu += delta_x_imu.segment<3>(18);// Update the camera states.auto cam_state_iter = state_server.cam_states.begin();for (int i = 0; i < state_server.cam_states.size();++i, ++cam_state_iter) {const VectorXd& delta_x_cam = delta_x.segment<6>(21+i*6);const Vector4d dq_cam = smallAngleQuaternion(delta_x_cam.head<3>());cam_state_iter->second.orientation = quaternionMultiplication(dq_cam, cam_state_iter->second.orientation);cam_state_iter->second.position += delta_x_cam.tail<3>();}

以上就完成了全部的状态更新过程，这周零零碎碎把这篇博客挤完了，对于MSCKF其实还只是了解了一个表面，例如MSCKF中能观性的处理、误差状态卡尔曼滤波的优化等，还有很多学问可以挖掘，之后再慢慢补充～

此外，对其他SLAM算法感兴趣的同学可以看考我的博客SLAM算法总结——经典SLAM算法框架总结