VINS-Mono(二)——优化问题建模

文章目录

Reference
视觉部分的优化问题建模
- 对第 i 帧的位姿进行求导
- 对第 j 帧的位姿进行求导
- 对逆深度 ρ \rho ρ进行求导
- 对IMU和Camera的外参进行求导
惯导部分的优化问题建模
- 误差部分对 i 时刻状态量的求导
- - 误差部分对于i 时刻位姿状态量的求导
  - 误差部分对于 i 时刻运动状态量的求导
- 误差部分对 j 时刻状态量的求导
- - 误差部分对于 j 时刻位姿状态量的求导
  - 误差部分对于 j 时刻运动状态量的求导
- 小结一下惯性部分的建模问题
小结

Reference

VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator.
Quaternion Kinematics for Error-State KF.

视觉部分的优化问题建模

视觉部分的问题就是一个BA问题，不过与纯视觉不同，这里涉及到两个坐标系之间的变换，分别是camera坐标系C和IMU坐标系（通常认为这个是机体坐标系）b，同时因为整个过程中使用的是逆深度表示空间中的Landmark，因此这个投影过程会变得稍微麻烦一些，但是整个过程还是比较清晰的。
{ P c i = f c i ρ P b i = R c b P c i + t c b P w = R b i w P b i + t b i w P b j = ( R b j w ) T ( P w − t b j w ) P c j = ( R c b ) T ( P b j − t c b ) = [ X j , Y j , Z j ] T p c j = [ X j Z j , Y j Z j ] T e r r o r i j = [ X j Z j − u j , Y j Z j − v j ] T (1) \begin{aligned} \begin{cases} P_{ci} &= \frac{f_{ci}}{\rho} \\ P_{bi} &= R^{b}_{c}P_{ci}+t^{b}_{c} \\ P_w &= R^{w}_{bi}P_{bi}+t^{w}_{bi} \\ P_{bj} &= (R^{w}_{bj})^T(P_w-t^{w}_{bj}) \\ P_{cj} &= (R^{b}_{c})^T(P_{bj}-t^{b}_{c}) = \left[ X_j, Y_j, Z_j \right]^T \\ p_{cj} &= \left[ \frac{X_j}{Z_j}, \frac{Y_j}{Z_j} \right]^T \\ error_{ij} &= \left[ \frac{X_j}{Z_j}-u_j, \frac{Y_j}{Z_j}-v_j \right]^T \end{cases} \end{aligned} \tag{1} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧PciPbiPwPbjPcjpcjerrorij=ρfci=RcbPci+tcb=RbiwPbi+tbiw=(Rbjw)T(Pw−tbjw)=(Rcb)T(Pbj−tcb)=[Xj,Yj,Zj]T=[ZjXj,ZjYj]T=[ZjXj−uj,ZjYj−vj]T(1)
于是可以逐个求解Jacobian矩阵了：

对第 i 帧的位姿进行求导

∂ e i j ∂ T b i w = [ ∂ e i j ∂ t b i w , ∂ e i j ∂ R b i w ] = ∂ e i j ∂ p c j ∂ p c j ∂ P c j ∂ P c j ∂ P b j ∂ P b j ∂ P w [ ∂ P w ∂ t b i w , ∂ P w ∂ R b i w ] = I 2 × 2 [ 1 Z j 0 − X j Z j 2 0 1 Z j − Y j Z j 2 ] 2 × 3 ( R c b ) 3 × 3 T ( R b j w ) 3 × 3 T [ I , − R b i w [ P b i ] × ] 3 × 6 (2) \begin{aligned} \frac{\partial{e_{ij}}}{\partial{T^{w}_{bi}}} &= \left[\frac{\partial{e_{ij}}}{\partial{t^{w}_{bi}}}, \frac{\partial{e_{ij}}}{\partial{R^{w}_{bi}}}\right] \\ &= \frac{\partial{e_{ij}}}{\partial{p_{cj}}}\frac{\partial{p_{cj}}}{\partial{P_{cj}}}\frac{\partial{P_{cj}}}{\partial{P_{bj}}} \frac{\partial{P_{bj}}}{\partial{P_{w}}} \left[\frac{\partial{P_w}}{\partial{t^{w}_{bi}}}, \frac{\partial{P_w}}{\partial{R^{w}_{bi}}}\right] \\ &= I_{2\times2}\begin{bmatrix}\frac{1}{Z_j} & 0 & -\frac{X_j}{Z^2_j} \\ 0 & \frac{1}{Z_j} & -\frac{Y_j}{Z^2_j}\end{bmatrix}_{2\times3} (R^{b}_{c})^T_{3\times3}(R^{w}_{bj})^T_{3\times3}\left[I, -R^{w}_{bi}[P_{bi}]_{\times} \right]_{3\times6} \end{aligned} \tag{2} ∂Tbiw∂eij=[∂tbiw∂eij,∂Rbiw∂eij]=∂pcj∂eij∂Pcj∂pcj∂Pbj∂Pcj∂Pw∂Pbj[∂tbiw∂Pw,∂Rbiw∂Pw]=I2×2⎣⎡Zj100Zj1−Zj2Xj−Zj2Yj⎦⎤2×3(Rcb)3×3T(Rbjw)3×3T[I,−Rbiw[Pbi]×]3×6(2)

对第 j 帧的位姿进行求导

∂ e i j ∂ T b j w = [ ∂ e i j ∂ t b j w , ∂ e i j ∂ R b j w ] = ∂ e i j ∂ p c j ∂ p c j ∂ P c j ∂ P c j ∂ P b j [ ∂ P b j ∂ t b j w , ∂ P b j ∂ R b j w ] = I 2 × 2 [ 1 Z j 0 − X j Z j 2 0 1 Z j − Y j Z j 2 ] 2 × 3 ( R c b ) 3 × 3 T [ − ( R b j w ) T , [ P b j ] × ] 3 × 6 (3) \begin{aligned} \frac{\partial{e_{ij}}}{\partial{T^{w}_{bj}}} &= \left[\frac{\partial{e_{ij}}}{\partial{t^{w}_{bj}}}, \frac{\partial{e_{ij}}}{\partial{R^{w}_{bj}}}\right] \\ &= \frac{\partial{e_{ij}}}{\partial{p_{cj}}}\frac{\partial{p_{cj}}}{\partial{P_{cj}}}\frac{\partial{P_{cj}}}{\partial{P_{bj}}} \left[\frac{\partial{P_{bj}}}{\partial{t^{w}_{bj}}}, \frac{\partial{P_{bj}}}{\partial{R^{w}_{bj}}}\right] \\ &= I_{2\times2}\begin{bmatrix}\frac{1}{Z_j} & 0 & -\frac{X_j}{Z^2_j} \\ 0 & \frac{1}{Z_j} & -\frac{Y_j}{Z^2_j}\end{bmatrix}_{2\times3} (R^{b}_{c})^T_{3\times3}\left[-(R^{w}_{bj})^T, [P_{bj}]_{\times} \right]_{3\times6} \end{aligned} \tag{3} ∂Tbjw∂eij=[∂tbjw∂eij,∂Rbjw∂eij]=∂pcj∂eij∂Pcj∂pcj∂Pbj∂Pcj[∂tbjw∂Pbj,∂Rbjw∂Pbj]=I2×2⎣⎡Zj100Zj1−Zj2Xj−Zj2Yj⎦⎤2×3(Rcb)3×3T[−(Rbjw)T,[Pbj]×]3×6(3)

对逆深度 ρ \rho ρ进行求导

∂ e i j ∂ ρ = ∂ e i j ∂ p c j ∂ p c j ∂ P c j ∂ P c j ∂ P b j ∂ P b j ∂ P w ∂ P w ∂ P b i ∂ P b i ∂ P c i ∂ P c i ∂ ρ = I 2 × 2 [ 1 Z j 0 − X j Z j 2 0 1 Z j − Y j Z j 2 ] 2 × 3 ( R c b ) 3 × 3 T ( R b j w ) 3 × 3 T ( R b i w ) 3 × 3 ( R c b ) 3 × 3 ( − f i ρ 2 ) 3 × 1 (4) \begin{aligned} \frac{\partial{e_{ij}}}{\partial{\rho}} &= \frac{\partial{e_{ij}}}{\partial{p_{cj}}}\frac{\partial{p_{cj}}}{\partial{P_{cj}}}\frac{\partial{P_{cj}}}{\partial{P_{bj}}}\frac{\partial{P_{bj}}}{\partial{P_{w}}}\frac{\partial{P_{w}}}{\partial{P_{bi}}}\frac{\partial{P_{bi}}}{\partial{P_{ci}}}\frac{\partial{P_{ci}}}{\partial{\rho}} \\ &= I_{2\times2}\begin{bmatrix}\frac{1}{Z_j} & 0 & -\frac{X_j}{Z^2_j} \\ 0 & \frac{1}{Z_j} & -\frac{Y_j}{Z^2_j}\end{bmatrix}_{2\times3} (R^{b}_{c})^T_{3\times3}(R^{w}_{bj})^T_{3\times3}(R^{w}_{bi})_{3\times3}(R^{b}_{c})_{3\times3}(-\frac{f_i}{\rho^2})_{3\times1} \end{aligned} \tag{4} ∂ρ∂eij=∂pcj∂eij∂Pcj∂pcj∂Pbj∂Pcj∂Pw∂Pbj∂Pbi∂Pw∂Pci∂Pbi∂ρ∂Pci=I2×2⎣⎡Zj100Zj1−Zj2Xj−Zj2Yj⎦⎤2×3(Rcb)3×3T(Rbjw)3×3T(Rbiw)3×3(Rcb)3×3(−ρ2fi)3×1(4)

对IMU和Camera的外参进行求导

∂ e i j ∂ T c b = [ ∂ e i j ∂ t c b , ∂ e i j ∂ R c b ] = ∂ e i j ∂ p c j ∂ p c j ∂ P c j ∂ P c j ∂ T c b = ∂ e i j ∂ p c j ∂ p c j ∂ P c j ( ∂ P c j ∂ T c b + ( R c b ) T ∂ P b j ∂ T c b ) = ∂ e i j ∂ p c j ∂ p c j ∂ P c j ( ∂ P c j ∂ T c b + ( R c b ) T ∂ P b j ∂ P w ∂ P w ∂ P b i ∂ P b i ∂ T c b ) = I 2 × 2 [ 1 Z j 0 − X j Z j 2 0 1 Z j − Y j Z j 2 ] 2 × 3 ( [ − ( R c b ) T , [ P c j ] × ] + ( R c b ) T ( R b j w ) T R b i w [ I , − R c b [ P c i ] × ] ) 3 × 6 = I 2 × 2 [ 1 Z j 0 − X j Z j 2 0 1 Z j − Y j Z j 2 ] 2 × 3 ( [ ( R c b ) T ( ( R b j w ) T R b i w − I ) , ( [ P c j ] × − ( R c b ) T ( R b j w ) T R b i w R c b [ P c i ] × ) ] ) 3 × 6 (5) \begin{aligned} \frac{\partial{e_{ij}}}{\partial{T^{b}_{c}}} &= \left[\frac{\partial{e_{ij}}}{\partial{t^{b}_{c}}}, \frac{\partial{e_{ij}}}{\partial{R^{b}_{c}}}\right] \\ &= \frac{\partial{e_{ij}}}{\partial{p_{cj}}}\frac{\partial{p_{cj}}}{\partial{P_{cj}}}\frac{\partial{P_{cj}}}{\partial{T^{b}_{c}}} \\ &= \frac{\partial{e_{ij}}}{\partial{p_{cj}}}\frac{\partial{p_{cj}}}{\partial{P_{cj}}}(\frac{\partial{P_{cj}}}{\partial{T^{b}_{c}}}+(R^{b}_{c})^T\frac{\partial{P_{bj}}}{\partial{T^{b}_{c}}}) \\ &= \frac{\partial{e_{ij}}}{\partial{p_{cj}}}\frac{\partial{p_{cj}}}{\partial{P_{cj}}}(\frac{\partial{P_{cj}}}{\partial{T^{b}_{c}}}+(R^{b}_{c})^T\frac{\partial{P_{bj}}}{\partial{P_{w}}}\frac{\partial{P_{w}}}{\partial{P_{bi}}}\frac{\partial{P_{bi}}}{\partial{T^{b}_{c}}}) \\ &= I_{2\times2}\begin{bmatrix}\frac{1}{Z_j} & 0 & -\frac{X_j}{Z^2_j} \\ 0 & \frac{1}{Z_j} & -\frac{Y_j}{Z^2_j}\end{bmatrix}_{2\times3}\left( \left[ -(R^{b}_{c})^T, [P_{cj}]_{\times} \right] + (R^{b}_{c})^T(R^{w}_{bj})^TR^{w}_{bi} \left[ I, -R^{b}_{c}[P_{ci}]_{\times} \right] \right)_{3\times6} \\ &= I_{2\times2}\begin{bmatrix}\frac{1}{Z_j} & 0 & -\frac{X_j}{Z^2_j} \\ 0 & \frac{1}{Z_j} & -\frac{Y_j}{Z^2_j}\end{bmatrix}_{2\times3}\left( \left[ (R^{b}_{c})^T((R^{w}_{bj})^TR^{w}_{bi}-I), ([P_{cj}]_{\times}-(R^{b}_{c})^T(R^{w}_{bj})^TR^{w}_{bi}R^{b}_{c}[P_{ci}]_{\times}) \right] \right)_{3\times6} \end{aligned} \tag{5} ∂Tcb∂eij=[∂tcb∂eij,∂Rcb∂eij]=∂pcj∂eij∂Pcj∂pcj∂Tcb∂Pcj=∂pcj∂eij∂Pcj∂pcj(∂Tcb∂Pcj+(Rcb)T∂Tcb∂Pbj)=∂pcj∂eij∂Pcj∂pcj(∂Tcb∂Pcj+(Rcb)T∂Pw∂Pbj∂Pbi∂Pw∂Tcb∂Pbi)=I2×2⎣⎡Zj100Zj1−Zj2Xj−Zj2Yj⎦⎤2×3([−(Rcb)T,[Pcj]×]+(Rcb)T(Rbjw)TRbiw[I,−Rcb[Pci]×])3×6=I2×2⎣⎡Zj100Zj1−Zj2Xj−Zj2Yj⎦⎤2×3([(Rcb)T((Rbjw)TRbiw−I),([Pcj]×−(Rcb)T(Rbjw)TRbiwRcb[Pci]×)])3×6(5)

在VINS-Mono的代码中，作者把 [ P c i ] × [P_{ci}]_{\times} [Pci]×给分解了，这块儿不太理解作者为什么要分解开，但是其实展开之后的结果是一样的。

惯导部分的优化问题建模

惯导部分的建模也是一个最小化误差的问题，该误差部分由两个部分组成，一部分是状态变量组成的帧间间隔，这部分与位置、角度、速度相关，与bias不相关：
{ p b j b i = ( R b i w ) T [ ( w p b j − w p b i ) − w v b i Δ t + 1 2 w g Δ t 2 ] v b j b i = ( R b i w ) T [ ( w v b j − w v b i ) + w g Δ t ] q b j b i = ( q b i w ) − 1 ⊗ ( q b j w ) b a j = b a i b g j = b g i (6) \begin{aligned} \begin{cases} p^{bi}_{bj} &= (R^{w}_{bi})^T \left[({^w}p_{bj}-{^{w}}p_{bi})-{^w}v_{bi}\Delta{t}+\frac{1}{2}{^{w}}g\Delta{t}^2\right] \\ v^{bi}_{bj} &= (R^{w}_{bi})^T\left[({^w}v_{bj} - {^w}v_{bi})+{^{w}}g\Delta{t}\right] \\ q^{bi}_{bj} &= (q^{w}_{bi})^{-1}\otimes(q^{w}_{bj}) \\ ba_{j} &= ba_{i} \\ bg_{j} &= bg_{i} \end{cases} \end{aligned} \tag{6} ⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧pbjbivbjbiqbjbibajbgj=(Rbiw)T[(wpbj−wpbi)−wvbiΔt+21wgΔt2]=(Rbiw)T[(wvbj−wvbi)+wgΔt]=(qbiw)−1⊗(qbjw)=bai=bgi(6)
另一部分是IMU的积分值 { p ‾ b j b i , v ‾ b j b i , q ‾ b j b i } \{\overline{p}^{bi}_{bj}, \overline{v}^{bi}_{bj}, \overline{q}^{bi}_{bj}\} {pbjbi,vbjbi,qbjbi}，这部分因为是在 b i bi bi系下的积分，因此与位置、角度、速度（均在世界坐标系下）无关，但是这部分与bias息息相关，所以这部分笔者认为不能叫观测值；

所以整个问题的误差为：
{ e p = p b j b i − p ‾ b j b i = ( R b i w ) T [ ( w p b j − w p b i ) − w v b i Δ t + 1 2 w g Δ t 2 ] − p ‾ b j b i = ( R b i w ) T w Δ p − p ‾ b j b i e q = 2 [ ( q ‾ b j b i ) − 1 ⊗ q b j b i ] 3 × 1 e v = v b j b i − v ‾ b j b i = ( R b i w ) T ( w v b j − w v b i + w g Δ t ) − v ‾ b j b i = ( R b i w ) T w Δ v − v ‾ b j b i e b a = b a j − b a i e b g = b g j − b g i (7) \begin{aligned} \begin{cases} e_p &= p^{bi}_{bj}-\overline{p}^{bi}_{bj}=(R^{w}_{bi})^T \left[({^w}p_{bj}-{^{w}}p_{bi})-{^w}v_{bi}\Delta{t}+\frac{1}{2}{^{w}}g\Delta{t}^2\right]-\overline{p}^{bi}_{bj} = (R^{w}_{bi})^T {^w}\Delta{p} - \overline{p}^{bi}_{bj} \\ e_q &= 2[ (\overline{q}^{bi}_{bj})^{-1} \otimes q^{bi}_{bj}]_{3\times1} \\ e_v &= v^{bi}_{bj}-\overline{v}^{bi}_{bj}=(R^{w}_{bi})^T({^w}v_{bj} - {^w}v_{bi}+{^{w}}g\Delta{t})-\overline{v}^{bi}_{bj} = (R^{w}_{bi})^T {^w}\Delta{v}-\overline{v}^{bi}_{bj} \\ e_{ba} &= ba_j - ba_i \\ e_{bg} &= bg_j - bg_i \end{cases} \end{aligned} \tag{7} ⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧epeqevebaebg=pbjbi−pbjbi=(Rbiw)T[(wpbj−wpbi)−wvbiΔt+21wgΔt2]−pbjbi=(Rbiw)TwΔp−pbjbi=2[(qbjbi)−1⊗qbjbi]3×1=vbjbi−vbjbi=(Rbiw)T(wvbj−wvbi+wgΔt)−vbjbi=(Rbiw)TwΔv−vbjbi=baj−bai=bgj−bgi(7)
我这里稍微有一个问题就是在代码中的姿态误差 e q e_q eq和论文中的公式并不相同，起初笔者还是比较在意这个事情的，但是后来个人认为只要整个推导过程中按照一个公式来就可以了。

所以根据公式（7）对状态变量进行求导（按照VINS-Mono中的组织形式）：

误差部分对 i 时刻状态量的求导

误差部分对于i 时刻位姿状态量的求导

∂ e ∂ T b i = [ ∂ e ∂ p b i , ∂ e ∂ θ b i ] = [ ∂ e p ∂ p b i ∂ e p ∂ θ b i ∂ e q ∂ p b i ∂ e q ∂ θ b i ∂ e v ∂ p b i ∂ e v ∂ θ b i ∂ e b a ∂ p b i ∂ e b a ∂ θ b i ∂ e b g ∂ p b i ∂ e b g ∂ θ b i ] = [ − ( R b i w ) T [ ( R b i w ) T Δ p ] × 0 − [ [ q i j ] L [ q ‾ j i ⊗ J b g q δ b g i ] R ] 3 × 3 0 [ ( R b i w ) T w Δ v ] × 0 0 0 0 ] (8) \begin{aligned} \frac{\partial{e}}{\partial{T_{bi}}} &= \left[\frac{\partial{e}}{\partial{p_{bi}}}, \frac{\partial{e}}{\partial{\theta_{bi}}} \right] \\ &= \begin{bmatrix} \frac{\partial{e_p}}{\partial{p_{bi}}} & \frac{\partial{e_p}}{\partial{\theta_{bi}}} \\ \frac{\partial{e_q}}{\partial{p_{bi}}} & \frac{\partial{e_q}}{\partial{\theta_{bi}}} \\ \frac{\partial{e_v}}{\partial{p_{bi}}} & \frac{\partial{e_v}}{\partial{\theta_{bi}}} \\ \frac{\partial{e_{ba}}}{\partial{p_{bi}}} & \frac{\partial{e_{ba}}}{\partial{\theta_{bi}}} \\ \frac{\partial{e_{bg}}}{\partial{p_{bi}}} & \frac{\partial{e_{bg}}}{\partial{\theta_{bi}}} \\ \end{bmatrix}\\ &= \begin{bmatrix} -(R^{w}_{bi})^T & [(R^{w}_{bi})^T \Delta{p}]_{\times} \\ 0 & -\left[ [q^{j}_{i}]_{\mathrm{L}} [\overline{q}^{i}_{j} \otimes \mathrm{J}^{q}_{bg}\delta{bg_{i}}]_{\mathrm{R}} \right]_{3\times3} \\ 0 & [(R^{w}_{bi})^T {^w}\Delta{v}]_{\times} \\ 0 & 0 \\ 0 & 0 \end{bmatrix}\\ \end{aligned} \tag{8} ∂Tbi∂e=[∂pbi∂e,∂θbi∂e]=⎣⎢⎢⎢⎢⎢⎢⎡∂pbi∂ep∂pbi∂eq∂pbi∂ev∂pbi∂eba∂pbi∂ebg∂θbi∂ep∂θbi∂eq∂θbi∂ev∂θbi∂eba∂θbi∂ebg⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎡−(Rbiw)T0000[(Rbiw)TΔp]×−[[qij]L[qji⊗Jbgqδbgi]R]3×3[(Rbiw)TwΔv]×00⎦⎥⎥⎥⎥⎥⎤(8)

这里简单推导一下姿态误差对姿态的求导：
e ( θ + δ θ ) = 2 [ ( q ‾ j i ) − 1 ⊗ [ ( q i w ⊗ q ( δ θ ) ) − 1 ⊗ q j w ] ] 3 × 1 = 2 [ ( q ‾ j i ) − 1 ⊗ q ( δ θ ) − 1 ⊗ q j i ] 3 × 1 = 2 [ [ ( q j i ) − 1 ⊗ q ( δ θ ) ⊗ q ‾ j i ) ] − 1 ] 3 × 1 = 2 [ [ [ q i j ] L [ q ‾ j i ] R [ 1 1 2 δ θ ] ] − 1 ] 3 × 1 = 2 [ [ q i j ] L [ q ‾ j i ] R [ 1 − 1 2 δ θ ] ] 3 × 1 = 2 [ [ q i j ] L [ q ‾ j i ] R [ 1 0 ] + [ q i j ] L [ q ‾ j i ] R [ 0 − 1 2 δ θ ] ] 3 × 1 = e ( θ ) + [ [ q i j ] L [ q ‾ j i ] R [ 0 − δ θ ] ] 3 × 1 \begin{aligned} e(\theta+\delta{\theta}) &= 2[(\overline{q}^{i}_{j})^{-1} \otimes [(q^{w}_{i}\otimes q(\delta{\theta}))^{-1}\otimes q^{w}_{j}]]_{3\times1} \\ &= 2[(\overline{q}^{i}_{j})^{-1} \otimes q(\delta{\theta})^{-1}\otimes q^{i}_{j}]_{3\times1} \\ &= 2[[(q^{i}_{j})^{-1}\otimes q(\delta{\theta}) \otimes \overline{q}^{i}_{j})]^{-1}]_{3\times1} \\ &= 2[[[q^{j}_{i}]_{\mathrm{L}}[\overline{q}^{i}_{j}]_{\mathrm{R}}\begin{bmatrix}1 \\ \frac{1}{2}\delta{\theta}\end{bmatrix}]^{-1}]_{3\times1} \\ &= 2[[q^{j}_{i}]_{\mathrm{L}}[\overline{q}^{i}_{j}]_{\mathrm{R}}\begin{bmatrix}1 \\ -\frac{1}{2}\delta{\theta}\end{bmatrix}]_{3\times1} \\ &= 2[[q^{j}_{i}]_{\mathrm{L}}[\overline{q}^{i}_{j}]_{\mathrm{R}}\begin{bmatrix}1 \\ 0\end{bmatrix}+[q^{j}_{i}]_{\mathrm{L}}[\overline{q}^{i}_{j}]_{\mathrm{R}}\begin{bmatrix} 0 \\ -\frac{1}{2}\delta{\theta} \end{bmatrix}]_{3\times1} \\ &= e(\theta)+[[q^{j}_{i}]_{\mathrm{L}}[\overline{q}^{i}_{j}]_{\mathrm{R}}\begin{bmatrix} 0 \\ -\delta{\theta} \end{bmatrix}]_{3\times1} \end{aligned} e(θ+δθ)=2[(qji)−1⊗[(qiw⊗q(δθ))−1⊗qjw]]3×1=2[(qji)−1⊗q(δθ)−1⊗qji]3×1=2[[(qji)−1⊗q(δθ)⊗qji)]−1]3×1=2[[[qij]L[qji]R[121δθ]]−1]3×1=2[[qij]L[qji]R[1−21δθ]]3×1=2[[qij]L[qji]R[10]+[qij]L[qji]R[0−21δθ]]3×1=e(θ)+[[qij]L[qji]R[0−δθ]]3×1
这部分其实看到中间取了一个求逆操作，主要是为了和VINS-Mono中的公式对上。

所以采用求导的原始定义有：
∂ e q ∂ θ = l i m e ( θ + δ θ ) − e ( θ ) δ θ = − [ [ q i j ] L [ q ‾ j i ] R ] 3 × 3 \frac{\partial{e_q}}{\partial{\theta}}=\mathrm{lim}\frac{e(\theta+\delta{\theta})-e(\theta)}{\delta{\theta}}=-[[q^{j}_{i}]_{\mathrm{L}}[\overline{q}^{i}_{j}]_{\mathrm{R}}]_{3\times3} ∂θ∂eq=limδθe(θ+δθ)−e(θ)=−[[qij]L[qji]R]3×3

误差部分对于 i 时刻运动状态量的求导

∂ e ∂ M b i = [ ∂ e ∂ v b i , ∂ e ∂ b a i , ∂ e ∂ b g i ] = [ ∂ e p ∂ v b i ∂ e p ∂ b a i ∂ e p ∂ b g i ∂ e q ∂ v b i ∂ e q ∂ b a i ∂ e q ∂ b g i ∂ e v ∂ v b i ∂ e v ∂ b a i ∂ e v ∂ b g i ∂ e b a ∂ v b i ∂ e b a ∂ b a i ∂ e b a ∂ b g i ∂ e b g ∂ v b i ∂ e b g ∂ b a i ∂ e b g ∂ b g i ] = [ − ( R b i w ) T Δ t − J b a i p − J b g i p 0 − J b a i q − [ [ q i j ⊗ q ‾ j i ] L ] 3 × 3 J b g i q − ( R b i w ) T − J b a i v − J b g i v 0 − I 0 0 0 − I ] (9) \begin{aligned} \frac{\partial{e}}{\partial{M_{bi}}} &= \left[\frac{\partial{e}}{\partial{v_{bi}}}, \frac{\partial{e}}{\partial{ba_i}}, \frac{\partial{e}}{\partial{bg_i}} \right] \\ &= \begin{bmatrix} \frac{\partial{e_p}}{\partial{v_{bi}}} & \frac{\partial{e_p}}{\partial{ba_i}} & \frac{\partial{e_p}}{\partial{bg_i}} \\ \frac{\partial{e_q}}{\partial{v_{bi}}} & \frac{\partial{e_q}}{\partial{ba_i}} & \frac{\partial{e_q}}{\partial{bg_i}} \\ \frac{\partial{e_v}}{\partial{v_{bi}}} & \frac{\partial{e_v}}{\partial{ba_i}} & \frac{\partial{e_v}}{\partial{bg_i}} \\ \frac{\partial{e_{ba}}}{\partial{v_{bi}}} & \frac{\partial{e_{ba}}}{\partial{ba_i}} & \frac{\partial{e_{ba}}}{\partial{bg_i}} \\ \frac{\partial{e_{bg}}}{\partial{v_{bi}}} & \frac{\partial{e_{bg}}}{\partial{ba_i}} & \frac{\partial{e_{bg}}}{\partial{bg_i}} \\ \end{bmatrix}\\ &= \begin{bmatrix} -(R^{w}_{bi})^T \Delta{t} & -J^{p}_{ba_i} & -J^{p}_{bg_i} \\ 0 & -J^{q}_{ba_i} & -[[q^{j}_{i}\otimes \overline{q}^{i}_{j}]_{\mathrm{L}}]_{3\times3}J^{q}_{bg_i} \\ -(R^{w}_{bi})^T & -J^{v}_{ba_i} & -J^{v}_{bg_i} \\ 0 & -I & 0 \\ 0 & 0 &-I \end{bmatrix}\\ \end{aligned} \tag{9} ∂Mbi∂e=[∂vbi∂e,∂bai∂e,∂bgi∂e]=⎣⎢⎢⎢⎢⎢⎢⎡∂vbi∂ep∂vbi∂eq∂vbi∂ev∂vbi∂eba∂vbi∂ebg∂bai∂ep∂bai∂eq∂bai∂ev∂bai∂eba∂bai∂ebg∂bgi∂ep∂bgi∂eq∂bgi∂ev∂bgi∂eba∂bgi∂ebg⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎡−(Rbiw)TΔt0−(Rbiw)T00−Jbaip−Jbaiq−Jbaiv−I0−Jbgip−[[qij⊗qji]L]3×3Jbgiq−Jbgiv0−I⎦⎥⎥⎥⎥⎤(9)

依旧简单推导一下姿态误差对角速度bias的求导过程：
e ( b g + δ b g ) = 2 [ ( q ‾ j i ⊗ [ 1 1 2 J b g q δ b g ] ) − 1 ⊗ q j i ] 3 × 1 = 2 [ [ 1 1 2 J b g q δ b g ] − 1 ( q ‾ j i ) − 1 ⊗ q j i ] 3 × 1 = 2 [ [ ( q j i ) − 1 ⊗ q ‾ j i ) ⊗ [ 1 1 2 J b g q δ b g ] ] − 1 ] 3 × 1 = 2 [ [ [ q i j ⊗ q ‾ j i ] L [ 1 1 2 J b g q δ b g ] ] − 1 ] 3 × 1 = 2 [ [ q i j ⊗ q ‾ j i ] L [ 1 − 1 2 J b g q δ b g ] ] 3 × 1 = 2 [ [ q i j ⊗ q ‾ j i ] L [ 1 0 ] ] 3 × 1 + 2 [ [ q i j ⊗ q ‾ j i ] L [ 0 − 1 2 J b g q δ b g ] ] 3 × 1 = e ( b g ) + [ [ q i j ⊗ q ‾ j i ] L [ 0 − J b g q δ b g ] ] 3 × 1 \begin{aligned} e(bg+\delta{bg}) &= 2[(\overline{q}^{i}_{j} \otimes \begin{bmatrix}1 \\ \frac{1}{2}J^{q}_{bg} \delta{bg}\end{bmatrix})^{-1} \otimes q^{i}_{j}]_{3\times1} \\ &= 2[\begin{bmatrix}1 \\ \frac{1}{2}J^{q}_{bg} \delta{bg}\end{bmatrix}^{-1}(\overline{q}^{i}_{j})^{-1} \otimes q^{i}_{j}]_{3\times1} \\ &= 2[[(q^{i}_{j})^{-1}\otimes \overline{q}^{i}_{j}) \otimes \begin{bmatrix}1 \\ \frac{1}{2}J^{q}_{bg} \delta{bg}\end{bmatrix} ]^{-1}]_{3\times1} \\ &= 2[[[q^{j}_{i} \otimes \overline{q}^{i}_{j}]_{\mathrm{L}}\begin{bmatrix}1 \\ \frac{1}{2}J^{q}_{bg} \delta{bg}\end{bmatrix} ]^{-1}]_{3\times1} \\ &= 2[[q^{j}_{i} \otimes \overline{q}^{i}_{j}]_{\mathrm{L}}\begin{bmatrix}1 \\ -\frac{1}{2}J^{q}_{bg} \delta{bg}\end{bmatrix}]_{3\times1} \\ &= 2[[q^{j}_{i} \otimes \overline{q}^{i}_{j}]_{\mathrm{L}}\begin{bmatrix}1 \\ 0 \end{bmatrix}]_{3\times1} + 2[[q^{j}_{i} \otimes \overline{q}^{i}_{j}]_{\mathrm{L}}\begin{bmatrix}0 \\ -\frac{1}{2}J^{q}_{bg} \delta{bg}\end{bmatrix}]_{3\times1} \\ &= e(bg) + [[q^{j}_{i} \otimes \overline{q}^{i}_{j}]_{\mathrm{L}}\begin{bmatrix}0 \\ -J^{q}_{bg} \delta{bg}\end{bmatrix}]_{3\times1} \end{aligned} e(bg+δbg)=2[(qji⊗[121Jbgqδbg])−1⊗qji]3×1=2[[121Jbgqδbg]−1(qji)−1⊗qji]3×1=2[[(qji)−1⊗qji)⊗[121Jbgqδbg]]−1]3×1=2[[[qij⊗qji]L[121Jbgqδbg]]−1]3×1=2[[qij⊗qji]L[1−21Jbgqδbg]]3×1=2[[qij⊗qji]L[10]]3×1+2[[qij⊗qji]L[0−21Jbgqδbg]]3×1=e(bg)+[[qij⊗qji]L[0−Jbgqδbg]]3×1
这部分很重要的一定是通过四元数的求逆法则将 δ b g \delta{bg} δbg转移到公式的最后，最后再通过四元数求逆其实就是把四元数的虚部部分添加负号的方法把求逆符号由公式外转到宫室内。

同样使用求导的定义有：
∂ e q ∂ b g = l i m e ( b g + δ b g ) − e ( b g ) δ b g = − [ [ q i j ⊗ q ‾ j i ] L ] 3 × 3 J b g q \frac{\partial{e_q}}{\partial{bg}}=\mathrm{lim}\frac{e(bg+\delta{bg})-e(bg)}{\delta{bg}}=-[[q^{j}_{i}\otimes \overline{q}^{i}_{j}]_{\mathrm{L}}]_{3\times3}J^{q}_{bg} ∂bg∂eq=limδbge(bg+δbg)−e(bg)=−[[qij⊗qji]L]3×3Jbgq

误差部分对 j 时刻状态量的求导

误差部分对于 j 时刻位姿状态量的求导

∂ e ∂ T b j = [ ∂ e ∂ p b j , ∂ e ∂ θ b j ] = [ ∂ e p ∂ p b j ∂ e p ∂ θ b j ∂ e q ∂ p b j ∂ e q ∂ θ b j ∂ e v ∂ p b j ∂ e v ∂ θ b j ∂ e b a ∂ p b j ∂ e b a ∂ θ b j ∂ e b g ∂ p b j ∂ e b g ∂ θ b j ] = [ ( R b i w ) T 0 0 [ [ ( q ‾ j i ⊗ J b g q δ b g i ) − 1 ⊗ q j i ] R ] 3 × 3 0 0 0 0 0 0 ] (10) \begin{aligned} \frac{\partial{e}}{\partial{T_{bj}}} &= \left[\frac{\partial{e}}{\partial{p_{bj}}}, \frac{\partial{e}}{\partial{\theta_{bj}}} \right] \\ &= \begin{bmatrix} \frac{\partial{e_p}}{\partial{p_{bj}}} & \frac{\partial{e_p}}{\partial{\theta_{bj}}} \\ \frac{\partial{e_q}}{\partial{p_{bj}}} & \frac{\partial{e_q}}{\partial{\theta_{bj}}} \\ \frac{\partial{e_v}}{\partial{p_{bj}}} & \frac{\partial{e_v}}{\partial{\theta_{bj}}} \\ \frac{\partial{e_{ba}}}{\partial{p_{bj}}} & \frac{\partial{e_{ba}}}{\partial{\theta_{bj}}} \\ \frac{\partial{e_{bg}}}{\partial{p_{bj}}} & \frac{\partial{e_{bg}}}{\partial{\theta_{bj}}} \\ \end{bmatrix}\\ &= \begin{bmatrix} (R^{w}_{bi})^T & 0 \\ 0 & \left[ \left[(\overline{q}^{i}_{j} \otimes \mathrm{J}^{q}_{bg}\delta{bg_{i}})^{-1} \otimes q^{i}_{j} \right]_{\mathrm{R}} \right]_{3\times3} \\ 0 & 0 \\ 0 & 0 \\ 0 & 0 \end{bmatrix}\\ \end{aligned} \tag{10} ∂Tbj∂e=[∂pbj∂e,∂θbj∂e]=⎣⎢⎢⎢⎢⎢⎢⎡∂pbj∂ep∂pbj∂eq∂pbj∂ev∂pbj∂eba∂pbj∂ebg∂θbj∂ep∂θbj∂eq∂θbj∂ev∂θbj∂eba∂θbj∂ebg⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎢⎡(Rbiw)T00000[[(qji⊗Jbgqδbgi)−1⊗qji]R]3×3000⎦⎥⎥⎥⎥⎥⎤(10)

简单推导一下姿态误差对姿态的求导：
e ( θ + δ θ ) = 2 [ ( q ‾ j i ) − 1 ⊗ ( q i w ) − 1 ⊗ q j w ⊗ q ( δ θ ) ] 3 × 1 = 2 [ ( q ‾ j i ) − 1 ⊗ q j i ⊗ [ 1 1 2 δ θ ] ] 3 × 1 = 2 [ [ ( q ‾ j i ) − 1 ⊗ q j i ] L [ 1 1 2 δ θ ] ] 3 × 1 = e ( θ ) + [ [ ( q ‾ j i ) − 1 ⊗ q j i ] L [ 0 δ θ ] ] 3 × 1 \begin{aligned} e(\theta+\delta{\theta}) &= 2[(\overline{q}^{i}_{j})^{-1} \otimes (q^{w}_{i})^{-1}\otimes q^{w}_{j}\otimes q(\delta{\theta})]_{3\times1} \\ &= 2[(\overline{q}^{i}_{j})^{-1}\otimes q^{i}_{j} \otimes \begin{bmatrix}1 \\ \frac{1}{2}\delta{\theta}\end{bmatrix}]_{3\times1} \\ &= 2[[(\overline{q}^{i}_{j})^{-1}\otimes q^{i}_{j}]_{\mathrm{L}} \begin{bmatrix}1 \\ \frac{1}{2}\delta{\theta}\end{bmatrix}]_{3\times1} \\ &= e(\theta) + [[(\overline{q}^{i}_{j})^{-1}\otimes q^{i}_{j}]_{\mathrm{L}} \begin{bmatrix}0 \\ \delta{\theta}\end{bmatrix}]_{3\times1} \end{aligned} e(θ+δθ)=2[(qji)−1⊗(qiw)−1⊗qjw⊗q(δθ)]3×1=2[(qji)−1⊗qji⊗[121δθ]]3×1=2[[(qji)−1⊗qji]L[121δθ]]3×1=e(θ)+[[(qji)−1⊗qji]L[0δθ]]3×1

误差部分对于 j 时刻运动状态量的求导

∂ e ∂ M b j = [ ∂ e ∂ v b j , ∂ e ∂ b a j , ∂ e ∂ b g j ] = [ ∂ e p ∂ v b j ∂ e p ∂ b a j ∂ e p ∂ b g j ∂ e q ∂ v b j ∂ e q ∂ b a j ∂ e q ∂ b g j ∂ e v ∂ v b j ∂ e v ∂ b a j ∂ e v ∂ b g j ∂ e b a ∂ v b j ∂ e b a ∂ b a j ∂ e b a ∂ b g j ∂ e b g ∂ v b j ∂ e b g ∂ b a j ∂ e b g ∂ b g j ] = [ 0 0 0 0 0 0 ( R b i w ) T 0 0 0 I 0 0 0 I ] (11) \begin{aligned} \frac{\partial{e}}{\partial{M_{bj}}} &= \left[\frac{\partial{e}}{\partial{v_{bj}}}, \frac{\partial{e}}{\partial{ba_j}}, \frac{\partial{e}}{\partial{bg_j}} \right] \\ &= \begin{bmatrix} \frac{\partial{e_p}}{\partial{v_{bj}}} & \frac{\partial{e_p}}{\partial{ba_j}} & \frac{\partial{e_p}}{\partial{bg_j}} \\ \frac{\partial{e_q}}{\partial{v_{bj}}} & \frac{\partial{e_q}}{\partial{ba_j}} & \frac{\partial{e_q}}{\partial{bg_j}} \\ \frac{\partial{e_v}}{\partial{v_{bj}}} & \frac{\partial{e_v}}{\partial{ba_j}} & \frac{\partial{e_v}}{\partial{bg_j}} \\ \frac{\partial{e_{ba}}}{\partial{v_{bj}}} & \frac{\partial{e_{ba}}}{\partial{ba_j}} & \frac{\partial{e_{ba}}}{\partial{bg_j}} \\ \frac{\partial{e_{bg}}}{\partial{v_{bj}}} & \frac{\partial{e_{bg}}}{\partial{ba_j}} & \frac{\partial{e_{bg}}}{\partial{bg_j}} \\ \end{bmatrix}\\ &= \begin{bmatrix} 0 & 0 & 0 \\ 0 & 0 & 0 \\ (R^{w}_{bi})^T & 0 & 0 \\ 0 & I & 0 \\ 0 & 0 & I \end{bmatrix}\\ \end{aligned} \tag{11} ∂Mbj∂e=[∂vbj∂e,∂baj∂e,∂bgj∂e]=⎣⎢⎢⎢⎢⎢⎢⎡∂vbj∂ep∂vbj∂eq∂vbj∂ev∂vbj∂eba∂vbj∂ebg∂baj∂ep∂baj∂eq∂baj∂ev∂baj∂eba∂baj∂ebg∂bgj∂ep∂bgj∂eq∂bgj∂ev∂bgj∂eba∂bgj∂ebg⎦⎥⎥⎥⎥⎥⎥⎤=⎣⎢⎢⎢⎢⎡00(Rbiw)T00000I00000I⎦⎥⎥⎥⎥⎤(11)

小结一下惯性部分的建模问题

这部分的误差模型其实由两个部分组成，一部分是状态量构建的帧间变化量（与位姿和速度直接相关），另一部分是IMU预积分得到的帧间变化量，这部分由于是以 i 时刻作为参考系，因此和位姿速度无直接关系，但是在预积分过程中和角速度计和角速度计的bias息息相关；
在预积分过程中，虽然使用了精度较高的中点法，但是整个预积分过程中并没有涉及到 j 时刻的零偏，或者说认为 j 时刻的零偏与 i 时刻是一样的，所以看到误差部分对 j 时刻状态的求导是比较简单的；

小结

建模这部分就先到这里，下部分关注的重点就是边缘化和优化问题的先验部分了。