MSCKF相关问题推导

MSCKF文章中没有给出的推导和一些疑问。

paper链接

这个逼怕是用的左手系哦！！！！！！

关于状态向量

这个逼的状态向量里面的四元数是从全局坐标系到IMU坐标系变换的$^I_Gq$，也就是IMU坐标系中全局坐标系的方向，也就是全局坐标系中IMU坐标系方向的逆

方向的true state为：$\bar{q} = \delta \bar{q} \otimes \hat{\bar{q}}$

导数为:$\dot{q} = \frac{1}{2} \omega \otimes q$，文章中的$\Omega(\omega)$证明了这个是左手系

Jacobian矩阵

相机状态的Jacobian矩阵计算，单位四元数的逆就是其共轭四元数
error state角度的Jacobian，导数第二步推导使用分配律，$R^C_I$从IMU坐标变换到相机坐标

$\hat{\bar{q}}^C_G = \bar{q}^C_I \otimes \hat{\bar{q}}^I_G$

$\delta \bar{q}^C_G \otimes \hat{\bar{q}}^C_G = \bar{q}^C_I \otimes \delta \bar{q}^I_G \otimes \hat{\bar{q}}^I_G$

$\delta \bar{q}^C_G \otimes \bar{q}^C_I = \bar{q}^C_I \otimes \delta \bar{q}^I_G$

$\delta \bar{q}^C_G = \bar{q}^C_I \otimes \delta \bar{q}^I_G \otimes {\bar{q}^C_I}^*$

$[0, 0, 0, 1] + \frac{1}{2}[\delta \theta ^C, 0] = \bar{q}^C_I \otimes \frac{1}{2}[\delta \theta ^I, 0] \otimes {\bar{q}^C_I}^* + [0, 0, 0, 1]$

$\frac{\partial \delta \theta ^C}{\partial \delta \theta ^I} = R^C_I$

error state坐标的Jacobian（我觉得文章里面推的不对！）

$\hat{p}^G_C = \hat{p}^G_I + {R^I_G}^T p^I_C$

$\delta p^G_C + p^G_C = \delta p^G_I + p^G_I + \left( \delta R^I_G R^I_G \right)^T p^I_C \quad \text{猜测文章中是：} {\delta R^I_G}^T {R^I_G}^T$

$\delta p^G_C = \delta p^G_I + {R^I_G}^T {\delta R^I_G}^T p^I_C - {R^I_G}^T p^I_C$

$\delta p^G_C = \delta p^G_I - {R^I_G}^T \left[\delta \theta ^I \right]_{\times} p^I_C \quad \text{由于：} \delta R^I_G = I + \left[\delta \theta ^I \right]_{\times}+ ...$

$\delta p^G_C = \delta p^G_I + {R^I_G}^T \left[ p^I_C \right]_{\times} \delta \theta ^I$

$\frac{\partial \delta p^G_C}{\partial \delta p^G_I} = I$

$\frac{\partial \delta p^G_C}{\partial \delta \theta ^I} = {R^I_G}^T \left[p^I_C \right]_{\times}$

Update

根据残差方程

$r = H \widetilde{X} + noise$

计算error state，先计算特征点重投影误差和Jacobian矩阵，然后再计算error state

计算特征点坐标

多个相机由feature约束，map: feature -> {camera}

特征点的像素

$z = \frac{1}{Z} \begin{bmatrix} X \\ Y \end{bmatrix} + n$

特征点在相机坐标系中的坐标有

$p_f = \begin{bmatrix} X \\ Y \\ Z\end {bmatrix} = R(p_f - p_C)$

使用最小二乘解得到特征点坐标

---

先使用首尾两帧三角交汇得到初始解，先通过像素坐标（归一化平面）获得投影方向（各自相机的坐标系）单位长度向量$d1$与$d2$，即

$(u,v,f) \rightarrow (\frac{u}{f}, \frac{v}{f}, 1) \rightarrow (u,v,f).normalize()$

然后获取相机$C_1$到$C_2$的旋转矩阵$R$与平移$t$，有

$x_1 d_1 = t + R^T x_2 d_2$

$t = d_1 x_1 - R^T d_2 x_2$

$\begin{bmatrix} d_1 & - R^T d_2 \end{bmatrix} \begin{bmatrix} x_1 \\ x_2 \end{bmatrix} = t$

则$x_1 d_1$为特征点在第一个相机的坐标系中的坐标，以此坐标为初始值再利用其他帧数据计算特征点坐标

---

逆深度参数： $(u,v,f) \rightarrow (u/f,v/f,1/f)$，逆深度的逆深度是本身
由计算得到的坐标与相机参数得到重投影误差

计算误差Jacobian

重投影误差线性化有

$r^{(j)}_i \simeq H^{(j)}_{X_i} \widetilde{X} + H^{(j)}_{f_i} {}^G\widetilde{P}_{f_i} + n^{(j)}_i$

其中

$H^{(j)}_{X_i} = \begin{bmatrix} 0_{2 \times 15} & 0_{2 \times 6} & \cdots & \underbrace{\begin{matrix} J^{(j)}_i \lfloor {}^{C_i}\hat{X}_{f_i} \times \rfloor & -J^{(j)}_i C({}_G^{C_i}\hat{\bar{q}}) \end{matrix}}_{Jacobian \ wrt \ pose \ i} & \cdots \end{bmatrix}$

与

$H^{(j)}_{f_i} = J^{(j)}_i C({}_G^{C_i}\hat{\bar{q}})$

且有

$J^{(j)}_i = \nabla_{ {C_i}_{\hat{p}_{f_j} } } z^{(j)}_i = \frac{1}{C_i \hat{Z}_j} \begin{bmatrix} 1 & 0 & - \frac{C_i\hat{X}_j}{C_i\hat{Z}_j} \\ 0 & 1 & - \frac{C_i\hat{Y}_j}{C_i\hat{Z}_j} \end{bmatrix}$

将$r^{(j)}$投影到$H^{(j)}_f$的左零空间，有

$r_o^{(j)} \simeq A^T H^{(j)}_X \widetilde{X} + A^T n^{(j)} = H^{(j)}_0 \widetilde{X}^{(j)} + n^{(j)}_0$

由于$H^{(j)}_f$是$2M_j \times 3$满秩矩阵，其左零空间维数为$2M_j - 3$

Update

由于Jacobian矩阵维度很高，因此考虑使用QR分解降低维度，然后使用标准的EKF更新状态和协方差矩阵