MSCKF3讲：后端理论推导（上）

  写在前面，这篇论文中C表示旋转矩阵，或者把q变为旋转矩阵的意思。

1 MSCKF中的状态变量

  下面G表示世界系，I表示IMU坐标系（作为body系），cam0表示左目相机系。关于坐标系表示的申明，下面这种表达方式${}_G^{I}R$等价于$R_{IG}$，是一种常见的转换关系。

① IMU状态:

(4+3+3+3+3)×1 = 16×1
$${\mathbf{X}}_{\mathrm{I}}={\left[\begin{array}{ccccc}{}_G^I{\bar{q}}^T& {\mathbf{b}}_g^T& {}^G{\mathbf{v}}_I^T& {\mathbf{b}}_a^T& {}^G{\mathbf{p}}_I^T\end{array}\right]}^T$$

• ${}_G^I{\bar{q}}^T$表示 the rotation from frame {G} to frame {I}，描述世界系到惯性系的旋转，用单位四元数表示，4维。因为是JPL，局部坐标系，所以是$q_{IG}$而不是$q_{GI}$
• ${}^G{\mathbf{p}}_I^T$和${}^G{\mathbf{v}}_I^T$表示位置和速度，即平移量，the IMU position and velocity with respect to {G},在世界系下表示。
• ${\mathbf{b}}_g^T$和${\mathbf{b}}_a^T$分别表示the biases affecting the gyroscope and accelerometer measurements，即陀螺仪和加速度计的偏差。

② cam0状态：

  利用滑动窗口来记录的，不只是一帧的相机姿态。at any time instant the EKF state vector comprises (i) the evolving IMU state (ii)a history of up to N max past poses of the camera
$${\mathbf{X}}_{\mathrm{C}}={\left[\begin{array}{c}{\mathbf{T}}_m^T\cdots {\mathbf{T}}_n^T\end{array}\right]}^T$$
  注意相机的每个T都是由${}^C{q}_G$和${}^G{\mathbf{p}}_{Cam0}$表示（C也是cam0，简单表示，(4+3)×1×N ）

③ IMU和cam0间状态关系

(4+3)×1
$${}_C^I{\mathbf{q}}^{\top }{}^I{\mathbf{p}}_C^{\top }$$

2 微分方程递推（数值解）

微分方程

$$\{\begin{array}{l}{y}^{\prime }(x)=f(x,y),x\in I=(a,b)\\ y\left(x_0\right)=y_0\end{array}$$

拉格朗日中值定理

$$\frac{y\left(x_{n+1}\right)-y\left(x_n\right)}{h}=y^{\prime }(x+\theta h)$$

欧拉法：本质一阶泰勒，线性近似求导

$$y\left(x_i+\Delta x\right)=y\left(x_i\right)+y^{\prime }\left(x_i\right)\Delta x+\frac{y^{\prime \prime }\left(x_i\right)}{2!}\Delta x^2+\cdots +\frac{y^{\left(n\right)}\left(x_i\right)}{n!}\Delta x^n+O\left(\Delta x^{n+1}\right)$$

$$\frac{y\left(x_i+\Delta x\right)-y\left(x_i\right)}{\Delta x}+O(\Delta x)=f\left(x_i,y\left(x_i\right)\right)$$

$$\{\begin{array}{l}{y}_{i+1}=y_i+\hslash f\left(x_i,y_i\right),i=0,1,\cdots n-1\\ y_0=y\left(x_0\right)\end{array}$$

四阶龙格库塔法

$$\{\begin{array}{l}{y}_{n+1}=y_n+\frac{h}{6}\left(k_1+2{\mathbf{k}}_2+2k_3+k_4\right)\\ k_1=f\left(x_n,y_n\right)\\ k_2=f\left(x_n+\frac{h}{2},y_n+\frac{h}{2}{k}_1\right)\\ k_3=f\left(x_n+\frac{\hslash }{2},y_n+\frac{\hslash }{2}{\mathbf{k}}_2\right)\\ {\mathbf{k}}_4=\mathbf{f}\left({\mathbf{x}}_n+\mathbf{h},{\mathbf{y}}_n+\mathbf{h}{\mathbf{k}}_3\right)\end{array}$$

3 IMU状态预测

  预测是对状态估计量state进行迭代预测，而不是误差状态向量Error State（用来更新），但协方差是根据误差状态的运动模型进行更新。

  每一帧的IMU观测数据，需要对$X_I$中的姿态${}_G^I{q}^T$、速度${}^G{v}_I^T$和位置${}^G{p}_I^T$进行预测，偏差$b_a,b_g$保持不变。

$$\begin{array}{rl}{\mathbf{b}}_g(t+\Delta t)& ={\mathbf{b}}_g(t),\\ {\mathbf{b}}_a(t+\Delta t)& ={\mathbf{b}}_a(t),\\ \mathbf{g}(t+\Delta t)& =\mathbf{g}(t).\end{array}$$
  这里相直接给出了离散时间下的IMU偏差的运动学方程，没有给出从连续时间到离散的推导（实际上因为偏置的导数是一个高斯过程，其均值是0，所以离散条件下，偏置保持不变）。

3.1 连续时间下IMU状态运动学方程（微分方程）

  这部分参考论文A Multi-State Constraint Kalman Filterfor Vision-aided Inertial Navigation之3.1节

① 状态（理想值）

注意下面式子描述了IMU状态真值的运动学变化，包括了均值和协方差

$$\begin{array}{rlrlrl}{}_G^I\dot{\bar{q}}(t)& =\frac{1}{2}\mathbf{\Omega }(\mathbf{\omega }(t){)}_G^I\bar{q}(t),& {\dot{\mathbf{b}}}_g(t)& ={\mathbf{n}}_{wg}(t)& & \\ {\dot{\mathbf{v}}}_I(t)& ={}^G\mathbf{a}(t),& {\dot{\mathbf{b}}}_a(t)& ={\mathbf{n}}_{wa}(t),& {\dot{\mathbf{p}}}_I(t)& ={}^G{\mathbf{v}}_I(t)\end{array}$$

  其中矩阵$\Omega (\mathbf{\omega })$
$$\mathbf{\Omega }(\mathbf{\omega })=\left[\begin{array}{cc}-\lfloor \mathbf{\omega }\times \rfloor & \mathbf{\omega }\\ -{\mathbf{\omega }}^T& 0\end{array}\right],\lfloor \mathbf{\omega }\times \rfloor =\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right]$$
  角速度${\omega }_m$和加速度$a_m$的测量值：
$$\begin{array}{rl}& {\omega }_m=\omega +\mathbf{C}{(}_G^I\bar{q}){\omega }_G+{\mathbf{b}}_g+{\mathbf{n}}_g\\ & {\mathbf{a}}_m=\mathbf{C}{(}_G^I\bar{q}){(}^G\mathbf{a}{-}^G\mathbf{g}+2\lfloor {\omega }_G\times {\rfloor }^G{\mathbf{v}}_I+\lfloor {\omega }_G\times {\rfloor }^2{\mathbf{p}}_I)+{\mathbf{b}}_a+{\mathbf{n}}_a\end{array}$$
  上面${\omega }_G$论文中指the IMU measurements incorporate the effects of the planet’s rotation受到行星自转的影响，但是大部分实现中都没有考虑这个！

  一般来说，如果把IMU放在车体中央，可以省略其它元素对加速度干扰，即加速度$a_m$可以表示为下式
$$\begin{array}{rl}& {\mathbf{a}}_m=\mathbf{C}{(}_G^I\bar{q}){(}^G\mathbf{a}{-}^G\mathbf{g})+{\mathbf{b}}_a+{\mathbf{n}}_a\end{array}$$

② 状态（测量值）

  对上面公式取均值之后，就可以得到IMU状态测量值运动学公式，注意高斯过程和高斯噪声的均值皆为0，所以下面偏置导数和噪声均值皆可置零。下面${\mathbf{C}}_{\hat{q}}=\mathbf{C}{(}_G^I\hat{\vec{q}}),\hat{\mathbf{a}}={\mathbf{a}}_m-{\hat{\mathbf{b}}}_a\mathrm{and}\hat{\mathbf{\omega }}={\mathbf{\omega }}_m-{\hat{\mathbf{b}}}_g-{\mathbf{C}}_{\hat{q}}{\mathbf{\omega }}_G.$

$$\begin{array}{c}{}_G^I\dot{\hat{\bar{q}}}=\frac{1}{2}\mathbf{\Omega }(\hat{\mathbf{\omega }}{)}_G^I\hat{\bar{q}},{\hat{\mathbf{b}}}_g=0_{3\times 1},\\ {}^G{\dot{\hat{\mathbf{v}}}}_I={\mathbf{C}}_{\hat{q}}^T\hat{\mathbf{a}}-2\lfloor {\mathbf{\omega }}_G\times \rfloor {}^G{\hat{\mathbf{v}}}_I-\lfloor {\mathbf{\omega }}_G\times \rfloor {}^{2G}{\hat{\mathbf{p}}}_I+{}^G\mathbf{g}\\ {\dot{\hat{\mathbf{b}}}}_a=0_{3\times 1},G{\dot{\hat{\mathbf{p}}}}_I=G{\hat{\mathbf{v}}}_I\end{array}$$

  代码实现并没有考虑行星自转以及加速度相关影响，本质上就是最简单的IMU运动模型，对应论文Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight公式（1）

3.2 离散时间下IMU状态运动学方程（差分方程）

  从连续到离散，不论是对于状态方程，还是误差状态方程，都很简单（比如状态方程中，如果只是使用欧拉法去进行积分，那么速度平移等公式就像是高中物理一般）

  这个过程也是卡尔曼滤波的第一步：预测均值（Propagation Equations）！

3.2.1 预测姿态${}_G^I{q}^T$

  论文Indirect Kalman Filter for 3D Attitude Estimation1.6节给出了具体推导，0阶和1阶的区别就是在于在单位时间内角速度是否变化。0阶不变，一直是$\omega (t_k)$，1阶用两个时间点取了均值
$$\bar{\mathbf{\omega }}=\frac{\mathbf{\omega }(t_{k+1})+\mathbf{\omega }(t_k)}{2}$$

  MSCKF中使用的是**0阶积分(假设角速度在单位时间内恒定不变)**，其中$\hat{\omega }={\omega }_m-{\hat{b}}_g$
$$\begin{array}{rl}& |\hat{\omega }|>10^{-5}{\text{ 时: }}_G^I\hat{q}(t+\Delta t)=\left(\cos \left(\frac{|\hat{\omega }|}{2}\Delta t\right)\cdot I_{4\times 4}+\frac{1}{|\hat{\omega }|}\sin \left(\frac{|\hat{\omega }|}{2}\Delta t\right)\cdot \Omega (\hat{\omega })\right){}_G^I\hat{q}(t)\\ & |\hat{\omega }|\le 10^{-5}{\text{ 时: }}_G^I\hat{q}(t+\Delta t)=\left(I_{4\times 4}-\frac{\Delta t}{2}\Omega (\hat{\omega })\right){}_G^I\hat{q}(t)\end{array}$$

  姿态${}_G^I{q}^T$的预测利用了角速度！其中$\hat{\omega }={\omega }_m-{\hat{b}}_g$。下标m表示测量值，测量值减去一个偏差表示理想值。（这里和高博新书表示略有不同，一般都是${\omega }_m=\hat{\omega }+{\hat{b}}_g$，即测量 = 理想值 + 偏差）。

3.2.2 预测速度${}^G{v}_I^T$和位置${}^G{p}_I^T$

  要注意的是，一个点在不同坐标系下的速度矢量并不相同，不是一个矢量在不同坐标系中的表达（参考高博新书）。我们一般说的速度，都是指机器人本身在世界系下的速度，其相对于自身坐标系，速度一直都是0。

  注意这里的速度和位置都是在世界系下的矢量，加速度是在IMU系下的量，所以要变换到世界系下，还有$g=(0,0,-9.8)$。下标m值测量值。
$$\begin{array}{c}\dot{\hat{\mathrm{p}}}(\mathrm{t})=\hat{\mathrm{v}}(\mathrm{t})\\ \dot{\hat{\mathrm{v}}}(\mathrm{t})=\hat{\mathrm{q}}(\mathrm{t})\hat{a}+\mathrm{g}\\ \hat{a}=a_m-{\hat{b}}_a\end{array}$$

  姿态的预测采用了比较简单的欧拉法，即认为角速度在单位时间内恒定不变。对于速度和位置的预测，采用了更加平滑的龙格库塔法！
$$\begin{array}{rl}& \hat{v}(t+\Delta t)=\hat{v}(t)+\frac{\Delta t}{6}\left(k_{v_1}+2k_{v_2}+2k_{v_3}+k_{v_4}\right)\\ & k_{v_1}=\hat{R}(t)\hat{a}+g\\ & k_{v_2}=\hat{R}(t+\Delta t/2)\hat{a}+g\\ & k_{v_3}=\hat{R}(t+\Delta t/2)\hat{a}+g\\ & k_{v_4}=\hat{R}(t+\Delta t)\hat{a}+g\\ & \hat{p}(t+\Delta t)=\hat{p}(t)+\frac{\Delta t}{6}\left(k_{p_1}+2k_{p_2}+2k_{p_3}+k_{p_4}\right)\\ & k_{p_1}=\hat{v}(t)\\ & k_{p_2}=\hat{v}(t)+k_{v_1}\Delta t/2\\ & k_{p_3}=\hat{v}(t)+k_{v_2}\Delta t/2\\ & k_{p_4}=\hat{v}(t)+k_{v_3}\Delta t\end{array}$$

  速度${}^G{v}_I^T$和位置${}^G{p}_I^T$的预测利用了加速度！

4 IMU误差状态预测

  IMU更新时用龙格库塔法得到了新的IMU状态，

  我们现在还要递推什么？递推IMU误差协方差矩阵。实际更新的算法使用了ESKF，维护的是误差状态，因此要推出新一时刻的误差状态协方差与上⼀时刻误差状态协方差及传感器误差协方差的关系。

  下面结论参考论文A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation。

  误差怎么表示？

  理想数值 = 估计数值 + 误差

4.1 连续时间下 误差状态

4.1.1 推导过程

① 旋转

  四元数的推导比较麻烦，参考Indirect Kalman Filter for 3D Attitude Estimation 2.4节 Continuous Time Error State Equations

② 速度

​ 区分清楚估计值就是利用测量值估计的一个数

③ 平移

  平移的导数即速度
$${}^G\delta {\dot{\mathbf{p}}}_I{=}^G\delta {\mathbf{v}}_I$$

④ 偏置

  IMU噪声模型中说的很清楚了，偏差的导数是高斯过程（0均值）。
$$\delta \dot{\mathbf{b}}={\mathbf{n}}_{\mathbf{w}}$$

4.1.2 结论

  写成矩阵形式
$$\dot{\delta \mathbf{x}}={\mathbf{F}}_c\cdot \delta \mathbf{x}+{\mathbf{G}}_c\cdot \mathbf{n}$$
  变量：
$${\delta \mathbf{x}=(\begin{array}{ccccccc}{}_G^I\delta {\mathbf{\theta }}^{\top }& \delta {\mathbf{b}}_g^{\top }& {}^G\delta {\mathbf{v}}_I^{\top }& \delta {\mathbf{b}}_a^{\top }& {}^G\delta {\mathbf{p}}_I^{\top }& {}_C^I\delta {\mathbf{\theta }}^{\top }& {}^I\delta {\mathbf{p}}_C^{\top }\end{array})}^{\top }$$

$${\mathbf{n}=(\begin{array}{cccc}{\mathbf{n}}_{\mathbf{g}}& {\mathbf{n}}_{\mathbf{wg}}& {\mathbf{n}}_{\mathbf{a}}& {\mathbf{n}}_{\mathbf{wa}}\end{array})}^{\top }$$

矩阵F注意点：

① 速度：即第三行，忽略了相关因素影响，3.1中①中描述了

② 外参：相机与IMU的转换，这个是不变的，所以第6、7行都是0

③ 这里只是5个变量，S-MSCKF论文以及代码中都还有外参两个自变量，所以F实际是21*21矩阵。7个变量，每个都是3×3

④ 参见Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight公式（2）

$$\mathbf{F}=\left(\begin{array}{ccccc}-\lfloor {\hat{\mathbf{\omega }}}_{\times }\rfloor & -{\mathbf{I}}_3& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ -C{\left({}_C^I\hat{\mathbf{q}}\right)}^{\top }\lfloor {\hat{\mathbf{a}}}_{\times }\rfloor & 0_{3\times 3}& 0_{3\times 3}& -C{\left({}_G^I\hat{\mathbf{q}}\right)}^{\top }& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& {\mathbf{I}}_3& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\end{array}\right)$$

​
$$\mathbf{G}=(\begin{array}{cccc}-{\mathbf{I}}_3& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& {\mathbf{I}}_3& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& -C{\left({}_G^I\hat{\mathbf{q}}\right)}^{\top }& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& {\mathbf{I}}_3\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\end{array})$$

补充：关于速度，考虑对于加速度a的各种影响，则F是下式，这里省略了6、7行的0矩阵，矩阵G是相同的。A Multi-State Constraint Kalman Filterfor Vision-aided Inertial Navigation公式（10）

$$\mathbf{F}=[\begin{array}{ccccc}-\lfloor \hat{\mathbf{\omega }}\times \rfloor & -{\mathbf{I}}_3& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ -{\mathbf{C}}_{\hat{q}}^T\lfloor \hat{\mathbf{a}}\times \rfloor & 0_{3\times 3}& -2\lfloor {\omega }_G\times \rfloor & -{\mathbf{C}}_{\hat{q}}^T& -\lfloor {\omega }_G\times {\rfloor }^2\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& {\mathbf{I}}_3& 0_{3\times 3}& 0_{3\times 3}\end{array}]$$

注意：我们并不会直接使用F和G去预测等等，而是在求解离散时间下协方差时利用。

4.2 离散时间下误差状态

  参考Indirect Kalman Filter for 3D Attitude Estimation 2.5节 Discrete Time Error State Equations

  对一个线性系统做离散化，是由固定的通解的（见线性系统理论笔记），如下
$$\delta \mathbf{x}(t+\Delta t)=\mathbf{\Phi }\left(t+\Delta t,t\right)\delta {\mathbf{x}}_t+{\int }_t^{t+\Delta t}\mathbf{\Phi }(t+\Delta t,\tau ){\mathbf{G}}_{\mathbf{c}}(\tau )\mathbf{n}(\tau )d\tau$$
  把一个连续系统离散化，在线性系统理论中讲过，系统矩阵$F$变为了 状态转移矩阵$ \Phi(t+\Delta t,t)= exp(Ft)$
$$\begin{array}{rl}\Phi (t+\Delta t,t)& =\exp \left({\mathbf{F}}_c\Delta t\right)\\ & ={\mathbf{I}}_{6\times 6}+{\mathbf{F}}_c\Delta t+\frac{1}{2!}{\mathbf{F}}_c^2\Delta t^2+\dots \end{array}$$
  其中幂指数（对应代码）：
$$\begin{array}{rl}& {\mathbf{F}}_c=\left[\begin{array}{cc}-\lfloor \hat{\mathbf{\omega }}\times \rfloor & -{\mathbf{I}}_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}\end{array}\right],{\mathbf{F}}_c^2=\left[\begin{array}{cc}\lfloor \hat{\mathbf{\omega }}\times {\rfloor }^2& \lfloor \hat{\mathbf{\omega }}\times \rfloor \\ 0_{3\times 3}& 0_{3\times 3}\end{array}\right]\\ & {\mathbf{F}}_c^3=\left[\begin{array}{cc}-\lfloor \hat{\mathbf{\omega }}\times {\rfloor }^3& -\lfloor \hat{\mathbf{\omega }}\times {\rfloor }^2\\ 0_{3\times 3}& 0_{3\times 3}\end{array}\right],{\mathbf{F}}_c^4=\left[\begin{array}{cc}\lfloor \hat{\mathbf{\omega }}\times {\rfloor }^4& \lfloor \hat{\mathbf{\omega }}\times {\rfloor }^3\\ 0_{3\times 3}& 0_{3\times 3}\end{array}\right]\end{array}$$
  代入通解，可得到：
$$\delta {\mathbf{x}}_{t+\Delta t}=({\mathbf{I}}_{6\times 6}+{\mathbf{F}}_c\Delta t+\frac{1}{2!}{\mathbf{F}}_c^2\Delta t^2+\dots )\delta {\mathbf{x}}_t+{\int }_t^{t+\Delta t}\mathbf{\Phi }(t+\Delta t,\tau ){\mathbf{G}}_{\mathbf{c}}(\tau )\mathbf{n}(\tau )d\tau$$

4.2.1 均值

  在MSCKF中，误差状态的均值实际上没有被使用，所以相关的论文里面也没有给出均值的推导，不过，把连续变为离散是一件较为容易的事情，参考状态，类似于下面形式（高博新书给出的，但具体肯定是有所差别的，看是否利用高阶的积分来推导，如龙格库塔法 ）
$$\begin{array}{rl}\delta \mathbf{p}(t+\Delta t)& =\delta \mathbf{p}+\delta \mathbf{v}\Delta t,\\ \delta \mathbf{\upsilon }(t+\Delta t)& =\delta \mathbf{v}+(-\mathbf{R}(\tilde{\mathbf{a}}-{\mathbf{b}}_a{)}^{\wedge }\delta \mathbf{\theta }-\mathbf{R}\delta {\mathbf{b}}_a+\delta \mathbf{g})\Delta t-{\mathbf{\eta }}_v,\\ \delta \mathbf{\theta }(t+\Delta t)& =\mathrm{Exp}\left(-(\tilde{\mathbf{\omega }}-{\mathbf{b}}_g)\Delta t\right)\delta \mathbf{\theta }-\delta {\mathbf{b}}_g\Delta t-{\mathbf{\eta }}_{\theta },\\ \delta {\mathbf{b}}_g(t+\Delta t)& =\delta {\mathbf{b}}_g+{\mathbf{\eta }}_{bg},\\ \begin{array}{r}\delta {\mathbf{b}}_a(t+\Delta t)\end{array}& =\delta {\mathbf{b}}_a+{\mathbf{\eta }}_{ba},\end{array}$$
​

4.2.2 协方差矩阵

  写在前面，其实这里的状态转移矩阵会经过可观测性约束进行一定的修改！.

   实际上是对离散化后的方程分两步计算，一个是前面状态转移矩阵线性协方差计算，再加上后面高斯噪声的协方差。（两个都符合高斯分布，所以协方差计算也符合）。-----这里也说明，虽然我们不用连续时间下的误差状态，但是会利用这个F和G矩阵，这也是为什么前面会求解。

$$\begin{array}{c}{\mathbf{P}}_{k+1|k}={\Phi }_k{\mathbf{P}}_{k|k}{\Phi }_k^T+{\mathbf{Q}}_{d,k}\\ {\Phi }_k=\Phi (t_{k+1},t_k)=\exp \left({\int }_{t_k}^{t_{k+1}}\mathbf{F}(\tau )d\tau \right)\\ {\mathbf{Q}}_{d,k}={\int }_{t_k}^{t_{k+1}}\mathbf{\Phi }\left(t_{k+1},\tau \right){\mathbf{G}}_c(\tau ){\mathbf{Q}}_c{\mathbf{G}}_c^{\mathrm{T}}(\tau ){\mathbf{\Phi }}^{\mathrm{T}}\left(t_{k+1},\tau \right)\mathrm{d}\tau \end{array}$$

  $Q_c$是噪声带来的协方差。噪声是⾼斯白噪声，且不同时刻是独立的（所以每个变量至只与自身相关，与其它无关，也就是协方差矩阵非对角线元素全是0），因此连续时间系统噪声协方差矩阵由
$$\begin{array}{c}{\mathbf{Q}}_c=E\left[\mathbf{n}(t+\tau ){\mathbf{n}}^{\mathrm{T}}(t)\right]=[\begin{array}{cccc}{\mathbf{N}}_{\mathrm{r}}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& {\mathbf{N}}_{\mathrm{wr}}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& {\mathbf{N}}_{\mathrm{a}}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& {\mathbf{N}}_{\mathrm{wa}}\end{array}]=\\ [\begin{array}{cccc}{\sigma }_{r_c}^2\cdot {\mathbf{I}}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& {\sigma }_{w_c}^2\cdot {\mathbf{I}}_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& {\sigma }_{a_c}^2\cdot {\mathbf{I}}_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& {\sigma }_{w_{ac}}^2\cdot {\mathbf{I}}_{3\times 3}\end{array}]\end{array}$$

• 实际上的协方差矩阵还有相机的影响
• 
  

  当下一个 IMU，即第 k+1 个 IMU 来到后，此时 k+1 时刻的整体协方差矩阵可写为
$${\mathbf{P}}_{k+1|k}=(\begin{array}{cc}{\mathbf{P}}_{I{I}_{k+1|k}}& {\mathbf{\Phi }}_k{\mathbf{P}}_{I{C}_{k|k}}\\ {\mathbf{P}}_{I{C}_{k|k}}^{\top }{\mathbf{\Phi }}_k^{\top }& {\mathbf{P}}_{C{C}_{k|k}}\end{array})$$

  递推阶段关于相机的加持都是0，也就是这个阶段关于相机状态部分是“不动”。但是公共部分是需要改变的，因为IMU部分变了
$${\mathbf{P}}_{k+1|k}=(\begin{array}{cc}{\mathbf{P}}_{I{I}_{k+1|k}}& {\mathbf{\Phi }}_k{\mathbf{P}}_{I{C}_{k|k}}\\ {\mathbf{P}}_{I{C}_{k|k}}^{\top }{\mathbf{\Phi }}_k^{\top }& {\mathbf{P}}_{C{C}_{k|k}}\end{array})$$

4.3 状态增广

4.3.1 什么是状态增广？

其次，搞明白什么是状态增广

状态增广：cam新来了⼀帧，状态跟协方差矩阵会有哪些变化

  在没有图像进来时，对IMU状态进行预测，并计算系统误差状态协方差矩阵；在有图像进来时，根据相机与IMU的相对外参计算当前相机的位姿。然后将最新的相机状态加入到系统状态向量中去，然后扩增误差状态协方差矩阵。

① 利用最新imu状态和外参计算当前相机位姿

$${{}_G^C\hat{\mathbf{q}}={}_I^C\hat{\mathbf{q}}\otimes {}_G^I\hat{\mathbf{q}},{}^G{\hat{\mathbf{p}}}_C={}^G{\hat{\mathbf{p}}}_C+C(\begin{array}{c}{}_G^I\hat{\mathbf{q}}\end{array})}^{\top }{}^I{\hat{\mathbf{p}}}_C$$

② 相机位姿分别对imu状态和外参的雅可比，扩展误差状态协方差
$$\begin{array}{c}{P}^{(21+6(N+1))\times (21+6(N+1))}=[\begin{array}{c}{I}_{21+6N}\\ J_{6\times (21+6N)}\end{array}]P^{(21+6N)\times (21+6N)}{\left[\begin{array}{c}{I}_{21+6N}\\ J_{6\times (21+6N)}\end{array}\right]}^T\\ =[\begin{array}{cc}P& P{J}^T\\ JP& JP{J}^T\end{array}]\end{array}$$

<1> 雅可比推导

  关于雅可比，维度6*（21+6N），6N是指对之前的外参的雅可比，全为0.行维表示$R_{cw}$和$t_{wc}$分别对${\tilde{\mathbf{x}}}_I={\left(\begin{array}{cccccccccccc}{}_G^I{\tilde{\mathbf{\theta }}}^{\top }& & {\tilde{\mathbf{b}}}_g^{\top }& & {}^G{\tilde{\mathbf{v}}}_I^{\top }& & {\tilde{\mathbf{b}}}_a^{\top }& & {}^G{\tilde{\mathbf{p}}}_I^{\top }& & {}^I{\tilde{\mathbf{\theta }}}_C^{\top }& {}^I{\tilde{\mathbf{p}}}_C^{\top }\end{array}\right)}^{\top }$的雅可比，故此共21列。可以分为两部分，如下公式所示

$$\mathbf{J}=\left(\begin{array}{cc}{\mathbf{J}}_I& 0_{6\times 6N}\end{array}\right)$$

原论文给出的公式如下，但是和代码对不上？

$${\mathbf{J}}_I=\left(\begin{array}{ccccc}C\left(\frac{I}{G}\hat{\mathbf{q}}\right)& 0_{3\times 9}& 0_{3\times 3}& {\mathbf{I}}_3& 0_{3\times 3}\\ -C{\left(\frac{I}{G}\hat{\mathbf{q}}\right)}^{\top }\lfloor {}^I{\hat{\mathbf{p}}}_{C\times }\rfloor & 0_{3\times 9}& {\mathbf{I}}_3& 0_{3\times 3}& {\mathbf{I}}_3\end{array}\right)$$

代码中应该是下面这样的

$$J=[\begin{array}{ccccccc}{R}_{cb}& 0& 0& 0& 0& \mathbf{I}& 0\\ (R_{wb}{t}_{bc}{)}^{\wedge }& 0& 0& 0& \mathbf{I}& 0& R_{wb}\end{array}]$$

下面求下具体的雅可比

<2> 扩展误差状态协方差

直接把雅可比乘进去就好

4.3.2 为什么要状态增广

  IMU Propagation只改变IMU状态向量和其对应的协方差，与相机无关；而Measurement Updata的观测模型是残差相对于相机状态的观测模型，与IMU状态没有直接关联。状态扩增就相当于相机和IMU状态之间的桥梁，通过关联协方差$P_{IC}$描述相机和IMU状态之间的关系，每一个相机状态都与IMU状态形成关联，这样在观测更新相机状态的同时，会间接更新IMU状态。

5 状态更新

5.1 理论分析

  理想数值 = 估计数值(预测的状态) + 误差 （误差状态协方差矩阵）。

  预测过程包括对名义状态的预测（IMU 积分）以及对误差状态的预测
$$\begin{array}{rl}\delta x_{\mathrm{pred}}& =F\delta x,\\ P_{\mathrm{pred}}& =FP{F}^{\top }+Q.\end{array}$$
  考虑更新过程。假设一个抽象的传感器能够对状态变量产生观测，其观测方程为抽象的 h，那么可以写为
$$z=h(x)+v,v\sim \mathcal{N}(0,V),$$

  在 ESKF 中，我们当前拥有名义状态 x 的估计以及误差状态 δx 的估计，且希望更新的是误差状态，因此要计算投影误差相对于误差状态的雅可比矩阵
$$\begin{gathered} H={\frac{\partial h}{\partial \delta \mathbf{x}}|}_{x_{\mathrm{pred}}} \\ =\frac{\partial \delta \mathbf{z}}{\partial \hat{\mathbf{z}}}\frac{\partial \hat{\mathbf{z}}}{\partial \hat{\mathbf{x}}}\frac{\partial \hat{\mathbf{x}}}{\partial \delta \mathrm{x}} \\ =\frac{\partial \hat{\mathbf{z}}}{\partial \hat{\mathbf{x}}}\frac{\partial \hat{\mathbf{x}}}{\partial \delta \mathrm{x}} \\ =\frac{\partial \hat{\mathbf{z}}}{\partial \hat{\mathbf{x}}} \end{gathered}$$

$\delta \mathbf{z}=\mathbf{z}-\hat{\mathbf{z}}$, $\delta \mathbf{z}$对z求导就是单位阵（观测都是正常加减法，不存在旋转矩阵那种乘法），同理，$\delta \mathbf{x}=\mathbf{x}-\hat{\mathbf{x}}$在处理旋转矩阵这种状态量是扰动乘法以外，求导也是单位阵！出于严谨，可以得到（对应状态p、v、R、b_g、b_a、g）

$$\begin{array}{r}\frac{\partial x}{\partial \delta x}=\mathrm{diag}(I_3,I_3,\frac{\partial \mathrm{Log}(\mathbf{R}(\mathrm{Exp}(\delta \mathbf{\theta })))}{\partial \delta \mathbf{\theta }},I_3,I_3,I_3).\end{array}$$

  相当于连乘旋转矩阵，需要用BCH近似计算，实际不应该直接作为单位阵，还有一点，这里如果是JPL四元数，那么应该是左扰动，与下面有区别。
$$\frac{\partial \mathrm{Log}(\mathbf{R}(\mathrm{Exp}(\delta \mathbf{\theta })))}{\partial \delta \mathbf{\theta }}={\mathbf{J}}_r^{-1}(\mathbf{R}).$$
  代码中貌似认为是小量，直接作为单位阵，即$H=\frac{\partial \hat{\mathbf{z}}}{\partial \hat{\mathbf{x}}}$

  再计算卡尔曼增益，进而计算误差状态的更新过程
$$\begin{array}{rlrl}& \mathbf{K}& & =P_{\text{pred}}{H}^{\top }(H{P}_{\text{pred}}{H}^{\top }+V{)}^{-1},\\ & \delta x& & =K(z-h({\mathbf{x}}_{\mathrm{pred}})),\\ & \mathbf{x}& & =x_{\mathrm{pred}}+\delta \mathbf{x},\\ & \text{P}& & =(\mathbf{I}-\mathbf{K}\mathbf{H}){\mathbf{P}}_{\mathrm{pred}}.\end{array}$$

说白了，ESKF就是预测的时候对状态和误差状态都预测了，但实际上只用了状态的预测值，对于误差状态，主要是利用了协方差和更新后的误差Δx。在更新这一块没有使用原始状态对整个方程进行更新！针对不同的观测模型，就有不同的雅可比矩阵H！这个是最重要的！

5.2 视觉观测雅可比矩阵H推导

  前面已经知道，H是计算投影误差相对于误差状态的雅可比矩阵

  滑动窗口内状态——IMU状态、左相机状态（旋转、平移）
$$\tilde{\mathbf{x}}={\left(\begin{array}{cccc}{\tilde{\mathbf{x}}}_I^{\top }& {\tilde{\mathbf{x}}}_{C_1}^{\top }& \dots & {\tilde{\mathbf{x}}}_{C_N}^{\top }\end{array}\right)}^{\top }$$
  由于MSCKF更新使用的是不在跟踪上的点，所以这个点必然在IMU状态前⾯，因此关于IMU状态的雅可比就是0，我们只需要计算关于相机状态的雅可比！

  单个路标点雅可比，论文公式（5）
$${\mathbf{r}}_i^j={\mathbf{z}}_i^j-{\hat{\mathbf{z}}}_i^j={\mathbf{H}}_{C_i}^j{\tilde{\mathbf{x}}}_{C_i}+{\mathbf{H}}_{f_i}^j{}^G{\tilde{\mathbf{p}}}_j+{\mathbf{n}}_i^j$$

下面推导本质就是纯视觉雅可比计算，可参考十四讲

当一个路标点看不见时，雅可比矩阵，论文公式（5）

$${\mathbf{r}}_i^j={\mathbf{z}}_i^j-{\hat{\mathbf{z}}}_i^j={\mathbf{H}}_{C_i}^j{\tilde{\mathbf{x}}}_{C_i}+{\mathbf{H}}_{f_i}^j{}^G{\tilde{\mathbf{p}}}_j+{\mathbf{n}}_i^j$$

  如果是双目，观测会翻倍，但下面M指观测到路标点的次数

当最新帧有多个路标点看不见时，论文公式（这里没有了路标下标i）

$${\mathbf{r}}^j={\mathbf{H}}_{\mathrm{x}}^j\tilde{\mathbf{x}}+{\mathbf{H}}_f^{jG}{\tilde{\mathbf{p}}}_j+{\mathbf{n}}^j$$

5.3 视觉观测雅可比的后续处理

5.3.1 左零空间投影

参考论文A Multi-State Constraint Kalman Filterfor Vision-aided Inertial Navigation公式（24）~（26）

  对于 EKF，残差线性化需要满足如下形式，即残差仅与一组状态向量的误差项成线性化关系$r\cong H\delta x+n$，且噪声项为与状态向量无关的零均值的高斯分布

  而 MSCKF 的的残差与两个状态向量的误差项相关，不满足上式的线性化形式，因此不能直接应用 EKF 的测量更新流程

  为了解决这个问题，并让这对路标点的雅可比隐式地参与到计算，进行左零空间投影！（具体参考后面的零空间实现，实际也是利用QR分解）

$$\begin{array}{rl}{r}_0^{(2M-3M_L)\times 1}& ={\mathrm{A}}^T{r}^{2M\times 1}\cong \underset{H_0}{\underbrace{{\mathrm{A}}^T{H}_X^{2M\times (15+6N)}}}{\tilde{X}}^{(15+6N)\times 1}+\underset{n_0}{\underbrace{{\mathrm{A}}^T{n}^{2M\times 1}}}\\ & ={H_0}^{(2M-3)\times (15+6N)}{\tilde{X}}^{(15+6N)\times 1}+{n_0}^{(2M-3)\times 1}\end{array}$$

5.3.2 QR分解降低计算量

参考论文A Multi-State Constraint Kalman Filterfor Vision-aided Inertial Navigation公式（27）~（30）

  投影完之后，我们就可以得到新的残差和相应的雅可比矩阵$H_0$。但是$H_0$矩阵通常维度很大，为了降低计算量，对$H_0$进行QR分解：

  QR分解会得到一个正交矩阵和一个上三角矩阵！正交矩阵的转置和其本身的乘积是单位矩阵，且该矩阵的所有列向量彼此正交（内积为0）
$${H_0}^{(2M-3)\times (15+6N)}=[Q_1{Q}_2]\left[\begin{array}{c}{T}_H\\ 0\end{array}\right]$$
  对上面新的残差左边乘以$[\begin{array}{c}{\mathbf{Q}}_1^T\\ {\mathbf{Q}}_2^T\end{array}]$,得到
$$\left[\begin{array}{c}{Q}_1^T{r}_0\\ Q_2^T{r}_0\end{array}\right]=\left[\begin{array}{c}{T}_H\\ 0\end{array}\right]\tilde{X}+\left[\begin{array}{c}{Q}_1^T{n}_0\\ Q_2^T{n}_0\end{array}\right]$$
  这个过程和左零空间投影很像，这里相当于取第一行对应的矩阵块，左零空间则是取第二行对应的矩阵块。总之，都是利用了QR分解！

5.4 更新

参考论文A Multi-State Constraint Kalman Filterfor Vision-aided Inertial Navigation公式（31）~（33）

6 左零空间投影

6.1 为什么要进行左零空间投影

要符合EKF的残差线性化形式

​  视觉slam中，得到重投影误差r之后，做更新前需要计算重投影误差相对于位姿与三维点的雅可比，利用雅可比来更新位姿以及三维点。

​  有⼀种模式只优化位姿，那是认为三维点精度很高，没有误差，即使这样，点还是会在后端优化。所以重投影误差中是包含着三维点的误差信息的，msckf的思想就是做不带三维点坐标的状态更新，但是并没有完全不考虑三维点的影响，这个思想是与边缘化⼀样，通过“移动”的手段。
​  在上面描述中出现了两个雅可比矩阵，⼀个是重投影误差相对于状态量的雅可比（位姿，速度，bias等⼀切参与重投影误差的量），它的行数等于误差维度，也就是2n个，n为点数，例如我们的双目msckf就是4n，因为⼀个点在左右目都有观测，列数等于状态量维度。重投影误差相对于三维点的雅可比同理。

​  现在为了将这部分不参与计算，并让这部分隐式地参与到计算，如何做？要注意这里并不能直接向上面⼀样把三维点固定死，因为三维点的误差是不能忽略的。

残差

$${\mathbf{r}}_i^j={\mathbf{z}}_i^j-{\hat{\mathbf{z}}}_i^j={\mathbf{H}}_{C_i}^j{\tilde{\mathbf{x}}}_{C_i}+{\mathbf{H}}_{f_i}^j{\tilde{\mathbf{p}}}_j+{\mathbf{n}}_i^j$$

  我们找到关于对特征雅可比$H_{f_i}^j$的左零空间$V$，ensure the uncertainty of ${\tilde{\mathbf{p}}}_j$does not affect the residual，确保哪些只计算一次保持不变的三维点（误差很大）影响到残差的计算！
$${\mathbf{r}}_o^j={\mathbf{V}}^{\top }{\mathbf{r}}^j={\mathbf{V}}^{\top }{\mathbf{H}}_{\mathbf{x}}^j\tilde{\mathbf{x}}+{\mathbf{V}}^{\top }{\mathbf{n}}^j={\mathbf{H}}_{\mathbf{x},o}^j\tilde{\mathbf{x}}+{\mathbf{n}}_o^j$$
  找到⼀个矩阵V乘在等式左右，将三维点的那项变成0矩阵，那么只需要更新状态即可，同时点也通过V矩阵融入到状态中，数学上讲通过A矩阵将前两项投影到了第三项的零空间上，概率上讲这种操作使得与三维点无关。

左零空间与零空间区别

1. 零空间（Null Space）：
   • 零空间，也称为核（kernel），是一个线性变换（或矩阵）中所有映射到零向量的向量组成的空间。
   • 对于矩阵 A，其零空间是$Ax=0$所有解的向量组成的空间
2. 左零空间（Left Null Space）：
   • 左零空间是一个矩阵的转置的零空间。
   • 对于矩阵 A，其左零空间是$y^{T}A=0$所有解的向量组成的空间

零空间维度 + 矩阵A的秩 = 矩阵维度n

6.2 如何计算零空间矩阵V

  对于上面两个雅可比，维度分别如下，4是因为双目，单目观测为2，所以双目是4.n表示了当前观测z被观测的帧数
$${{\mathbf{H}}_{C_i}^j=(\begin{array}{c}{\mathbf{J}}_1{\mathbf{H}}_1\\ {\mathbf{J}}_2{\mathbf{R}}^{\top }{\mathbf{H}}_1\end{array})}_{4n\ast 6},{\mathbf{H}}_{f_i}^j={\left(\begin{array}{c}{\mathbf{J}}_1{\mathbf{H}}_2\\ {\mathbf{J}}_2{\mathbf{R}}^{\top }{\mathbf{H}}_2\end{array}\right)}_{4n\ast 3}$$

  把这个雅可比通过QR分解分出两个矩阵，正交矩阵 Q 和一个上三角矩阵 R 的乘积，然后对两个矩阵分块处理，可以得到类似下图矩阵块：

  因为Q是正交矩阵，所以Q2中任意一列乘以Q1中一列为0！所以Q2的转置实际上就是我们要找的左零空间矩阵（注意到一个正交矩阵Q乘以正交矩阵的转置就是单位阵！）
$$\begin{array}{c}\delta \mathbf{r}={\mathbf{J}}_{\mathbf{x}}\delta \mathbf{X}+{\mathbf{J}}_{\mathbf{p}}\delta \mathbf{P}\\ \delta \mathbf{r}={\mathbf{J}}_{\mathbf{x}}\delta \mathbf{X}+[\begin{array}{llc}{\mathbf{Q}}_1& & {\mathbf{Q}}_2\end{array}]\left[\begin{array}{l}{\mathbf{R}}_1\\ 0\end{array}\right]\delta \mathbf{P}\\ [\begin{array}{l}{{\mathbf{Q}}_1}^T\\ {{\mathbf{Q}}_2}^T\end{array}]\delta \mathbf{r}=\left[\begin{array}{l}{{\mathbf{Q}}_1}^T\\ {{\mathbf{Q}}_2}^T\end{array}\right]{\mathbf{J}}_{\mathbf{x}}\delta \mathbf{X}+\left[\begin{array}{l}{\mathbf{R}}_1\\ 0\end{array}\right]\delta \mathbf{P}\end{array}$$
  在说下为什么Q2就是左零空间矩阵呢？看下面矩阵，实际上可以分为两行，如果单独拿出Q2对应的哪一行，关于路标点的那一部分对应是0！