Ⅰ. 状态向量与目标函数

状态向量共包括滑动窗口内的 $n+1$ 个所有 IMU 状态$\mathbf{x}_k,k\in[0,n]$（包括位置、朝向、速度、加速度计 $bias$ 和陀螺仪 $bias$）、 Camera 到 IMU 的外参 $\mathbf{x}^b_c$、 $m+1$ 个 3D 点的逆深度$\lambda_l,l\in[0,m]$：

$$ \begin{align*} \mathcal{X}&=[\mathbf{x}_0,\mathbf{x}_1,···,\mathbf{x}_n,\mathbf{x}^b_c,\lambda_0,\lambda_1,···,\lambda_m]\\ \mathbf{x}_k&=[\mathbf{p}^w_{b_k},\mathbf{v}^w_{b_k},\mathbf{q}^w_{b_k},\mathbf{b}_a,\mathbf{b}_g],k\in[0,n]\\ \mathbf{x}^b_c&=[\mathbf{p}^b_c,\mathbf{q}^b_c] \end{align*}\tag{1} $$

$n$ 是关键帧的总数，$m$ 是滑动窗口中的特征总数。 $λ_l$ 是第 $l$ 个特征与其第一次观察的逆深度。

使用 BA 公式，我们最小化所有测量残差的先验和马氏距离之和，以获得最大后验估计为：

$$ \underset{\mathcal{X} }{\min} \left\{ \underbrace {\parallel \mathbf{r}_p-\mathbf{H}_p\mathcal{X} \parallel^2}_{边缘化的先验信息} +\underbrace {\sum_{k\in\mathcal{B} }\parallel\mathbf{r}_{\mathcal{B} }(\hat{\mathbf{z} }^{b_k}_{b_{k+1} },\mathcal{X}) \parallel^2_{\mathbf{P}^{b_k}_{b_{k+1} } } }_{IMU 测量残差} +\underbrace {\sum_{(l,j)\in\mathcal{C} }\rho(\parallel\mathbf{r}_\mathcal{C}(\hat{\mathbf{z} }^{c_j}_{l},\mathcal{X}) \parallel^2_{\mathbf{P}^{c_j}_l })}_{视觉的重投影残 差} \right\}\tag{2} $$

其中 $\mathbf{r}_{\mathcal{B} }(\hat{\mathbf{z} }^{b_k}_{b_{k+1} },\mathcal{X})$ 和 $\mathbf{r}_\mathcal{C}(\hat{\mathbf{z} }^{c_j}_{l},\mathcal{X})$ 分别是 IMU 和视觉测量的残差。 $\mathcal{B}$ 是所有 IMU 测量值的集合，$\mathcal{C}$ 是在当前滑动窗口中至少观察到两次的特征集合。 $\{\mathbf{r}_p, \mathbf{H}_p\}$ 是来自边缘化的先验信息。 三种残差都是用马氏距离表示。

Ⅱ. IMU 约束

残差

两帧之间的 PVQ 和 $bias$ 的变化量的差 :

$$ \mathbf{r}_{\mathcal{B} }(\hat{\mathbf{z} }^{b_k}_{b_{k+1} },\mathcal{X})= \begin{bmatrix} \delta {\alpha}^{b_k}_{b_{k+1} }\\ \delta {\theta}^{b_k}_{b_{k+1} }\\ \delta {\beta}^{b_k}_{b_{k+1} }\\ \delta {b}_{a}\\\delta {b}_{g} \end{bmatrix}= \begin{align*} \begin{bmatrix} \mathbf{R}^{b_k}_w(\mathbf{p}^w_{b_{k+1} }-\mathbf{p}^w_{b_k}+ \frac{1}{2}g^w\Delta t_k^2-\mathbf{v}^w_{b_k}\Delta t_k)-\hat{\alpha}^{b_k}_{b_{k+1} }\\ 2[{\mathbf{q}^w_{b_k}}^{-1}\otimes\mathbf{q}^w_{b_{k+1} } \otimes(\hat{\gamma}^{b_k}_{b_{k+1} })^{-1}]_{xyz}\\ \mathbf{R}^{b_k}_w(\mathbf{v}^w_{b_{k+1} }-\mathbf{v}^w_{b_k}+g^w\Delta t_k)-\hat{\beta}^{b_k}_{b_{k+1} }\\ \mathbf{b}_{ab_{k+1} }-\mathbf{b}_{ab_k}\\ \mathbf{b}_{wb_{k+1} }-\mathbf{b}_{wb_k}\\ \end{bmatrix} \end{align*}\tag{3} $$

其中各增量关于 bias 的 Jacobian 可从公式 $(4)$ 的 大 Jacobian 中的相应位置获得。 上面与其中各增量关于 $bias$ 的 Jacobian 可从公式 $(4)$ 的大 Jacobian 中的相应位置获得。

$$ {\mathbf{J}_{k+1} }^{15\times15}=\mathbf{F}\mathbf{J}_{k}\tag{4} $$

优化变量

待优化变量主要为两个时刻的 PVQ 以及 $bais$:

$$ \begin{align*} k时刻&： [\mathbf{p}^w_{b_k},\mathbf{q}^w_{b_k}],[\mathbf{v}^w_{b_k},\mathbf{b}_{a_k},\mathbf{b}_{w_k}]\\ k+1时刻&：[\mathbf{p}^w_{b_{k+1} },\mathbf{q}^w_{b_{k+1} }],[\mathbf{v}^w_{b_{k+1} },\mathbf{b}_{a_{k+1} },\mathbf{b}_{w_{k+1} }]\\ \end{align*} $$

Jacobian

计算 Jacobian 时，残差对应的求偏导对象为上面的优化变量，但是计算时采用扰动方式计算，即扰动为 ：

$$ \begin{align*} k时刻&： [\delta\mathbf{p}^w_{b_k},\delta\theta^w_{b_k}],[\delta\mathbf{v}^w_{b_k},\delta\mathbf{b}_{a_k},\delta\mathbf{b}_{w_k}]\\ k+1时刻&：[\delta\mathbf{p}^w_{b_{k+1} },\delta\theta^w_{b_{k+1} }],[\delta\mathbf{v}^w_{b_{k+1} },\delta\mathbf{b}_{a_{k+1} },\delta\mathbf{b}_{w_{k+1} }]\\ \end{align*} $$

IMU约束的雅可比：

$$ \begin{align*} \mathbf{J}&= \begin{bmatrix} \mathbf{J}[0]&\mathbf{J}[1]\\\mathbf{J}[2]&\mathbf{J}[3] \end{bmatrix}\\ &=\begin{bmatrix} [\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{p}^w_{b_k} } ,\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{q}^w_{b_k} }] &[\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{v}^w_{b_k} } ,\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{b}_{a_k} } ,\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{b}_{w_k} }]\\ [\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{p}^w_{b_{k+1} } } ,\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{q}^w_{b_{k+1} } }] &[\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{v}^w_{b_{k+1} } } ,\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{b}_{a_{k+1} } } ,\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{b}_{w_{k+1} } }] \end{bmatrix} \end{align*} $$

$$ \mathbf{J}[0]^{15\times7}= [\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{p}^w_{b_k} } ,\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{q}^w_{b_k} }] =\begin{bmatrix} -\mathbf{R}^{b_k}_w& \left[\mathbf{R}^{b_k}_w\left(\mathbf{p}^w_{b_{k+1} }-\mathbf{p}^w_{b_k}-\mathbf{v}^w_{b_k}\Delta t_k+ \frac{1}{2}g^w\Delta t_k^2\right) \right]^{\land}\\ 0&-\mathcal{R} \left[{\mathbf{q}_{b_k}^w}^{-1}\otimes \mathbf{q}^w_{b_{k+1} } \right]_{3\times3} \mathcal{L} \left[\hat{\gamma}^{b_k}_{b_{k+1} }\right]_{3\times3}\\ 0&\left[\mathbf{R}^{b_k}_w(\mathbf{v}^w_{b_{k+1} }-\mathbf{v}^w_{b_k}+g^w\Delta t_k)\right]^{\land}\\ 0&0\\ 0&0\\ \end{bmatrix}\tag{J0} $$

• 推导 $\mathbf{J}[0]$

定义：$\mathbf{J}[0]_{[i,j]}$ 表示矩阵 $\mathbf{J}[0]$ 的 第 $i$ 行 第 $j$ 列；

首先是 $\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{p}^w_{b_k} }$，显然，由公式 $(3)$，对于 $\mathbf{r}_{\mathcal{B} }$ 而言，只有 $\delta {\alpha}^{b_k}_{b_{k+1} }$ 对 $\mathbf{p}^w_{b_k}$ 求偏导才有值，其他为0，这就组成 $(J0)$ 里矩阵的第一列；

对于矩阵的 $\mathbf{J}[0]_{[2,1]}$ 和 $\mathbf{J}[0]_{[2,3]}$，可以使用旋转矩阵的左乘扰动模型来求：

这里为了书写方便，令 $\mathcal{P}^w=\mathbf{p}^w_{b_{k+1} }-\mathbf{p}^w_{b_k}+
\frac{1}{2}g^w\Delta t_k^2-\mathbf{v}^w_{b_k}\Delta t_k$

$$ \begin{align*} \frac{\partial \delta {\alpha}^{b_k}_{b_{k+1} } }{\partial \mathbf{p}^w_{b_k} } &=\frac{\partial \left[\mathbf{R}^{b_k}_w\mathcal{P}^w-\hat{\alpha}^{b_k}_{b_{k+1} }\right]}{\partial \mathbf{p}^w_{b_k} }\\ &=\underset{\varphi\rightarrow0}{\lim} \frac{(\mathbf{R}^{b_k}_w\exp(\varphi^{\land}))^{-1}\mathcal{P}^w }{\delta\varphi}\\ &\approx\underset{\varphi\rightarrow0}{\lim} \frac{(I-\varphi^{\land})\exp(\phi^{\land})\mathcal{P}^w-\exp(\phi^{\land})\mathcal{P} }{\delta\varphi}\\ &=\underset{\varphi\rightarrow0}{\lim} -\frac{\varphi^{\land}\mathbf{R}^{b_k}_w\mathcal{P}^w }{\delta\varphi}\\ &=\underset{\varphi\rightarrow0}{\lim} \frac{[\mathbf{R}^{b_k}_w\mathcal{P}^w]^{\land}\varphi }{\delta\varphi}\\ &=[\mathbf{R}^{b_k}_w\mathcal{P}^w]^{\land} \end{align*}\tag{J0-1} $$

因此，对于$\mathbf{J}[0]_{[2,1]}$ 和 $\mathbf{J}[0]_{[2,3]}$，他们仅仅是 $\mathcal{P}^w$ 不同，推导过程都是一样的。

关键是对于$\mathbf{J}[0]_{[2,2]}$ 的推导，是比较关键的，对应的雅可比为 $\frac{\partial \delta {\theta}^{b_k}_{b_{k+1} } }{\partial \mathbf{q}^w_{b_k} }$：

$$ \begin{align*} \frac{\partial \delta {\theta}^{b_k}_{b_{k+1} } }{\partial \mathbf{q}^w_{b_k} } &=\frac{\partial 2[{\mathbf{q}^w_{b_k}}^{-1}\otimes\mathbf{q}^w_{b_{k+1} } \otimes(\hat{\gamma}^{b_k}_{b_{k+1} })^{-1}]_{xyz} }{\partial \mathbf{q}^w_{b_k} }\\ &=2\underset{\delta {\theta}^{b_k}_{b_{k+1} }\rightarrow0}{\lim} \frac{ \left(\mathbf{q}_{b_k}^w\otimes \begin{bmatrix}1\\\frac{\delta\theta^{w}_{b_k} }{2} \end{bmatrix} \right)^{-1}\otimes \mathbf{q}^w_{b_{k+1} } \otimes(\hat{\gamma}^{b_k}_{b_{k+1} })^{-1} -\left(\mathbf{q}_{b_k}^w\otimes \begin{bmatrix}1\\0\end{bmatrix} \right)^{-1}\otimes \mathbf{q}^w_{b_{k+1} } \otimes(\hat{\gamma}^{b_k}_{b_{k+1} })^{-1} }{\delta {\theta}^{w}_{b_k} }\\ &=2\underset{\delta {\theta}^{b_k}_{b_{k+1} }\rightarrow0}{\lim} \frac{ \begin{bmatrix}1\\-\frac{\delta\theta^{w}_{b_k} }{2} \end{bmatrix} \otimes{\mathbf{q}_{b_k}^w}^{-1}\otimes \mathbf{q}^w_{b_{k+1} }\otimes(\hat{\gamma}^{b_k}_{b_{k+1} })^{-1} -\begin{bmatrix}1\\0 \end{bmatrix}\otimes {\mathbf{q}_{b_k}^w}^{-1}\otimes \mathbf{q}^w_{b_{k+1} }\otimes(\hat{\gamma}^{b_k}_{b_{k+1} })^{-1} }{\delta {\theta}^{w}_{b_k} }\\ &=2\underset{\delta {\theta}^{b_k}_{b_{k+1} }\rightarrow0}{\lim} \frac{ \left( \begin{bmatrix}1\\-\frac{\delta\theta^{w}_{b_k} }{2} \end{bmatrix} -\begin{bmatrix}1\\0 \end{bmatrix} \right) \otimes {\mathbf{q}_{b_k}^w}^{-1}\otimes \mathbf{q}^w_{b_{k+1} }\otimes(\hat{\gamma}^{b_k}_{b_{k+1} })^{-1} }{\delta {\theta}^{w}_{b_k} }\\ &=\underset{\delta {\theta}^{b_k}_{b_{k+1} }\rightarrow0}{\lim} \frac{ \begin{bmatrix}0\\-\delta\theta^{w}_{b_k} \end{bmatrix} \otimes {\mathbf{q}_{b_k}^w}^{-1}\otimes \mathbf{q}^w_{b_{k+1} }\otimes(\hat{\gamma}^{b_k}_{b_{k+1} })^{-1} }{\delta {\theta}^{w}_{b_k} }\\ &=\underset{\delta {\theta}^{b_k}_{b_{k+1} }\rightarrow0}{\lim} \frac{ \mathcal{R}\left[{\mathbf{q}_{b_k}^w}^{-1}\otimes \mathbf{q}^w_{b_{k+1} } \right] \mathcal{R}\left[(\hat{\gamma}^{b_k}_{b_{k+1} })^{-1} \right] \otimes\begin{bmatrix}0\\-\delta\theta^{w}_{b_k} \end{bmatrix} }{\delta {\theta}^{w}_{b_k} }\\ &=-\mathcal{R}\left[{\mathbf{q}_{b_k}^w}^{-1}\otimes \mathbf{q}^w_{b_{k+1} } \right]\mathcal{R}\left[(\hat{\gamma}^{b_k}_{b_{k+1} })^{-1} \right]\\ \end{align*}\tag{J0-2} $$

对于四元数的左右乘，在第二节的$\mathrm{(IA-3)}$ 提到过，这里再次作展示：

我们设四元数 $\mathbf{q}=[x,y,z,s]$为 $\mathbf{q}=[w,s]$，则：

$$ \begin{align*} \mathcal{R}(\mathbf{q}_a)&=\Omega(w)+sI_{4\times4} =\begin{bmatrix}-w^{\land}&w\\-w^T&0\end{bmatrix}+sI_{4\times4}\\ \mathcal{L}(\mathbf{q}_a)&=\Psi(w)+sI_{4\times4} =\begin{bmatrix}w^{\land}&w\\-w^T&0\end{bmatrix}+sI_{4\times4} \end{align*}\tag{5} $$

则有：

$$ \begin{align*} \mathcal{R}(\mathbf{q}^{-1})=\Omega(-w)+sI_{4\times4} &=\begin{bmatrix}w^{\land}&-w\\w^T&0\end{bmatrix}+sI_{4\times4}\\ \mathcal{L}(\mathbf{q}^{-1})=\Omega(-w)+sI_{4\times4} &=\begin{bmatrix}-w^{\land}&-w\\w^T&0\end{bmatrix}+sI_{4\times4}\\ \end{align*} \tag{6} $$

如果我们只取左上角的 $3\times3$ 虚部部分，则有：

$$ \begin{align*} \mathcal{R}(\mathbf{q}^{-1})=sI_{3\times3}+w^{\land}=\mathcal{L}(\mathbf{q})_{3\times3}\\ \mathcal{L}(\mathbf{q}^{-1})=sI_{3\times3}-w^{\land}=\mathcal{R}(\mathbf{q})_{3\times3} \end{align*} \tag{7} $$

故而 $\mathrm{(J0-2)}$ 可以整理如下：

$$ \begin{align*} \frac{\partial \delta {\theta}^{b_k}_{b_{k+1} } }{\partial \mathbf{q}^w_{b_k} } &=-\mathcal{R}\left[{\mathbf{q}_{b_k}^w}^{-1}\otimes \mathbf{q}^w_{b_{k+1} } \right]\mathcal{R}\left[(\hat{\gamma}^{b_k}_{b_{k+1} })^{-1} \right]\\ &=-\mathcal{R} \left[{\mathbf{q}_{b_k}^w}^{-1}\otimes \mathbf{q}^w_{b_{k+1} } \right]_{3\times3} \mathcal{L} \left[\hat{\gamma}^{b_k}_{b_{k+1} }\right]_{3\times3} \end{align*}\tag{J0-3} $$

证毕！

$$ \mathbf{J}[1]^{15\times9}= [\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{v}^w_{b_k} } ,\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{b}_{a_k} } ,\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{b}_{w_k} }] =\begin{bmatrix} -\mathbf{R}^{b_k}_w\Delta t&-\mathbf{J}^{\alpha}_{b_a}&-\mathbf{J}^{\alpha}_{b_w}\\ 0&0&-\mathcal{L}\left[{\mathbf{q}_{b_k}^w}^{-1}\otimes\mathbf{q}^w_{b_{k+1} } \otimes\hat{\gamma}^{b_k}_{b_{k+1} } \right]_{3\times3}\mathbf{J}^{\gamma}_{b_w}\\ -\mathbf{R}^{b_k}_w&-\mathbf{J}^{\beta}_{b_a}&-\mathbf{J}^{\beta}_{b_w}\\ 0&-\mathbf{I}&0\\ 0&0&-\mathbf{I} \end{bmatrix}\tag{J1} $$

• 推导 $\mathbf{J}[1]$

首先第一列的求导都是直接可以看出来的，这里不做推导。

由于在进行预积分的时候，我们使用的是给定的零偏进行建模，而零偏也是待优化的变量，为了避免在更新零偏的时候重新进行预积分操作，论文中，使用 $\alpha^{b_k}_{b_{k+1} },\beta^{b_k}_{b_{k+1} },\gamma^{b_k}_{b_{k+1} }$ 关于偏差的一阶近似进行补偿，可以写成：

$$ \begin{align*} \alpha^{b_k}_{b_{k+1} }&\approx \hat{\alpha}^{b_k}_{b_{k+1} }+\mathbf{J}^{\alpha}_{b_a}\delta b_{a_k}+\mathbf{J}^{\alpha}_{b_w}\delta b_{w_k} \\ \beta^{b_k}_{b_{k+1} }&\approx \hat{\beta}^{b_k}_{b_{k+1} }+\mathbf{J}^{\beta}_{b_a}\delta b_{a_k}+\mathbf{J}^{\beta}_{b_w}\delta b_{w_k} \\ \gamma^{b_k}_{b_{k+1} }&\approx \hat{\gamma}^{b_k}_{b_{k+1} }\otimes \begin{bmatrix}1\\\frac{1}{2}\mathbf{J}^{\gamma}_{b_w}\delta b_{w_k} \end{bmatrix} \end{align*}\tag{8} $$

其中 $\mathbf{J}^{\alpha}_{b_a}$ 和 是 $\mathbf{J}^{\alpha}_{b_{k+1} }$ 中的子块矩阵，其位置对应于 $\frac{\delta\alpha^{b_k}_{b_{k+1} } }{\delta b_{a_k} }$ 。 $\mathbf{J}^{\alpha}_{b_w}、\mathbf{J}^{\beta}_{b_a}、\mathbf{J}^{\beta}_{b_w}、\mathbf{J}^{\gamma}_{b_w}$ 也使用相同的含义。当偏差的估计发生轻微变化时，我们使用 $(8)$ 来近似地校正预积分结果，而不是重新传播。

这里主要对 $\mathbf{J[1]}_{[2,3]}$ 进行推导，对应的雅可比为 $\frac{\partial \delta\theta^{b_k}_{b_{k+1}} }{\partial\mathbf{q}^{w}_{k+1} }$:

$$ \begin{align*} \frac{\partial \delta\theta^{b_k}_{b_{k+1}} }{\partial b_{w_k} } &=\frac{\partial 2[{\mathbf{q}^{b_k}_w}^{-1}\otimes\mathbf{q}^w_{b_{k+1} } (\hat{\gamma}^{b_k}_{b_{k+1} })^{-1}]_{xyz} }{\partial b_{w_k} }\\ &=2\underset{\delta b_{w_k}\rightarrow0}{\lim} \frac{ {\mathbf{q}_{b_k}^w}^{-1}\otimes \mathbf{q}^w_{b_{k+1} }\otimes {\left[ \hat{\gamma}^{b_k}_{b_{k+1} }\otimes \begin{bmatrix}1\\\frac{1}{2}\mathbf{J}^{\gamma}_{b_w}\delta b_{w_k} \end{bmatrix} \right]}^{-1}- {\mathbf{q}_{b_k}^w}^{-1}\otimes \mathbf{q}^w_{b_{k+1} }\otimes {\left[ \hat{\gamma}^{b_k}_{b_{k+1} }\otimes \begin{bmatrix}1\\0 \end{bmatrix} \right]}^{-1} }{\delta b_{w_k}}\\ &=2\underset{\delta b_{w_k}\rightarrow0}{\lim} \frac{ {\mathbf{q}_{b_k}^w}^{-1}\otimes \mathbf{q}^w_{b_{k+1} }\otimes {\begin{bmatrix}0\\-\frac{1}{2}\mathbf{J}^{\gamma}_{b_w}\delta b_{w_k} \end{bmatrix} }\otimes{\hat{\gamma}^{b_k}_{b_{k+1} } }^{-1} }{\delta b_{w_k}}\\ &=\underset{\delta b_{w_k}\rightarrow0}{\lim} \frac{ {\mathbf{q}_{b_k}^w}^{-1}\otimes \mathbf{q}^w_{b_{k+1} }\otimes {\begin{bmatrix}0\\-\mathbf{J}^{\gamma}_{b_w}\delta b_{w_k} \end{bmatrix} }\otimes{\hat{\gamma}^{b_k}_{b_{k+1} } }^{-1} }{\delta b_{w_k}}\\ &=\underset{\delta b_{w_k}\rightarrow0}{\lim} \frac{ \mathcal{L}\left[{\mathbf{q}_{b_k}^w}^{-1}\otimes\mathbf{q}^w_{b_{k+1} } \right]\otimes \mathcal{R}\left[{\hat{\gamma}^{b_k}_{b_{k+1} } }^{-1} \right]\otimes {\begin{bmatrix}0\\-\mathbf{J}^{\gamma}_{b_w}\delta b_{w_k} \end{bmatrix} } }{\delta b_{w_k}}\\ &=-\mathcal{L}\left[{\mathbf{q}_{b_k}^w}^{-1}\otimes\mathbf{q}^w_{b_{k+1} } \right] \mathcal{R}\left[{\hat{\gamma}^{b_k}_{b_{k+1} } }^{-1} \right]\mathbf{J}^{\gamma}_{b_w}\\ &=-\mathcal{L}\left[{\mathbf{q}_{b_k}^w}^{-1}\otimes\mathbf{q}^w_{b_{k+1} } \right]_{3\times3} \mathcal{L}\left[\hat{\gamma}^{b_k}_{b_{k+1} } \right]_{3\times3} \mathbf{J}^{\gamma}_{b_w}\\ &=-\mathcal{L}\left[{\mathbf{q}_{b_k}^w}^{-1}\otimes\mathbf{q}^w_{b_{k+1} } \otimes\hat{\gamma}^{b_k}_{b_{k+1} } \right]_{3\times3}\mathbf{J}^{\gamma}_{b_w} \end{align*}\tag{J1-1} $$

证毕！

$$ \mathbf{J}[2]^{15\times7}= [\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{p}^w_{b_{k+1} } } ,\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{q}^w_{b_{k+1} } }] =\begin{bmatrix} \mathbf{R}^{b_k}_w&0\\ 0&\mathcal{L}\left[{\mathbf{q}^{b_k}_w}^{-1}\otimes \mathbf{q}^w_{b_{k+1} }\otimes\hat{\gamma}^{b_k}_{b_{k+1} }\right]_{3\times3}\\ 0&0\\ 0&0\\ 0&0\\ \end{bmatrix}\tag{J2} $$

• 推导 $\mathbf{J}[2]$

主要是对 $\mathbf{J}[2]_{[2,2]}$ 进行推导，即 $\frac{\partial \delta\theta^{b_k}_{b_{k+1}} } {\partial \mathbf{q}^w_{b_{k+1} } }$：

$$ \begin{align*} \frac{\partial \delta\theta^{b_k}_{b_{k+1}} } {\partial \mathbf{q}^w_{b_{k+1} } } &=\frac{\partial 2[{\mathbf{q}^{b_k}_w}^{-1}\otimes\mathbf{q}^w_{b_{k+1} } \otimes(\hat{\gamma}^{b_k}_{b_{k+1} })^{-1}]_{xyz} }{\partial \mathbf{q}^w_{b_{k+1} } }\\ &=2\underset{\delta\theta^w_{b_{k+1} }\rightarrow0}{\lim} \frac{ {\mathbf{q}^{b_k}_w}^{-1}\otimes \mathbf{q}^w_{b_{k+1} }\otimes \begin{bmatrix}1\\\frac{\delta\theta^w_{b_{k+1} } }{2}\end{bmatrix}\otimes (\hat{\gamma}^{b_k}_{b_{k+1} })^{-1} -{\mathbf{q}^{b_k}_w}^{-1}\otimes \mathbf{q}^w_{b_{k+1} }\otimes \begin{bmatrix}1\\0\end{bmatrix}\otimes (\hat{\gamma}^{b_k}_{b_{k+1} })^{-1} }{\delta\theta^w_{b_{k+1} } }\\ &=\underset{\delta\theta^w_{b_{k+1} }\rightarrow0}{\lim} \frac{ {\mathbf{q}^{b_k}_w}^{-1}\otimes \mathbf{q}^w_{b_{k+1} }\otimes \begin{bmatrix}0\\\delta\theta^w_{b_{k+1} }\end{bmatrix}\otimes (\hat{\gamma}^{b_k}_{b_{k+1} })^{-1} }{\delta\theta^w_{b_{k+1} } }\\ &=\underset{\delta\theta^w_{b_{k+1} }\rightarrow0}{\lim} \frac{ \mathcal{L}[{\mathbf{q}^{b_k}_w}^{-1}\otimes \mathbf{q}^w_{b_{k+1} }] \mathcal{R}[(\hat{\gamma}^{b_k}_{b_{k+1} })^{-1} ] \begin{bmatrix}0\\\delta\theta^w_{b_{k+1} }\end{bmatrix} }{\delta\theta^w_{b_{k+1} } }\\ &=\mathcal{L}\left[{\mathbf{q}^{b_k}_w}^{-1}\otimes \mathbf{q}^w_{b_{k+1} }\right] \mathcal{R}\left[(\hat{\gamma}^{b_k}_{b_{k+1} })^{-1} \right]\\ &=\mathcal{L}\left[{\mathbf{q}^{b_k}_w}^{-1}\otimes \mathbf{q}^w_{b_{k+1} }\right]_{3\times3} \mathcal{L}\left[(\hat{\gamma}^{b_k}_{b_{k+1} }) \right]_{3\times3}\\ &=\mathcal{L}\left[{\mathbf{q}^{b_k}_w}^{-1}\otimes \mathbf{q}^w_{b_{k+1} }\otimes\hat{\gamma}^{b_k}_{b_{k+1} }\right]_{3\times3} \end{align*} $$

证毕！

$$ \mathbf{J}[3]^{15\times9}= [\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{v}^w_{b_{k+1} } } ,\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{b}_{a_{k+1} } } ,\frac{\partial \mathbf{r}_{\mathcal{B} } }{\partial \mathbf{b}_{w_{k+1} } }] =\begin{bmatrix} 0&0&0\\ 0&0&0\\ \mathbf{R}^{b_k}_w&0&0\\ 0&\mathbf{I}&0\\ 0&0&\mathbf{I}\\ \end{bmatrix}\tag{J3} $$

这个不需要证，应该可以直接看出来。

上面公式在代码中对应： class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9>
对于 Evaluate 输入 double const *const *parameters, parameters[0], parameters[1], parameters[2],parameters[3] 分别对应 $4$ 个输入参数, 它们的长度依次是 $7,9,7,9$，分别对应 $4$ 个优化变量的
参数块。

协方差

代码 IMUFactor::Evaluate() 中 residual 还乘以一个信息矩阵 sqrt_info，这是因为真正的优化项其实是 马氏距离：$d = \mathbf{r}^T\mathbf{P}^{-1}\mathbf{r}$， $\mathbf{P}$ 是协方差， 又因为 Ceres 只接受最小二乘优化， 也就是 $\min(e^Te)$, 所以把 $\mathbf{P}$ 做 LLT 分解， 即 $𝐿𝐿^T= \mathbf{P}^{-1}$, 则有：

$$ d=\mathbf{r}^TLL^T\mathbf{r}=(L^T\mathbf{r})^T(L^T\mathbf{r})\tag{9} $$

令 $\mathbf{r}^{\prime} = L^T\mathbf{r}$ 作为新的优化误差, 这样就能用 Ceres 求解了。马氏距离距离其实相当于一个残差加权, 协方差大的加权小, 协方差小的加权大, 着重优化那些比较确定的残差。若写成 “sqrt_info.setIdentity()” 相当于不加权。

其中，$\mathbf{P}$ 是协方差，由之前提出的递推公式得到：

协方差的迭代公式为：

$$ {\mathbf{P}^{b_k}_{k+1} }^{15\times15} =\mathbf{F}\mathbf{P}^{b_k}_{k}\mathbf{F}^T+\mathbf{V}\mathbf{Q}\mathbf{V}^T\tag{10} $$

其中， 初始值 $\mathbf{P}^{b_k}_{b_k}=0$。 $\mathbf{Q}$ 为表示噪声项的对角协方差矩阵：

$$ \mathbf{Q}^{18\times18}= \begin{bmatrix} \sigma^2_{a}&0&0&0&0&0\\ 0&\sigma^2_{w}&0&0&0&0\\ 0&0&\sigma^2_{a}&0&0&0\\ 0&0&0&\sigma^2_{w}&0&0\\ 0&0&0&0&\sigma^2_{b_a}&0\\ 0&0&0&0&0&\sigma^2_{b_w}\\ \end{bmatrix}\tag{11} $$