VINS-Mono预积分公式推导
写在前面
最近看了ESKF之后，收获蛮大的，特别是对于四元数的残差的推导，感觉给出了一个很标准的推导方式，因此就趁热打铁，把VINS中的预积分部分残差推导了一下，也算是对上了VINS中imu_factor.h中的代码部分。
残差
设状态变量为 x = [ P i ， P j ， V i ， V j ， Q i ， Q j ， B a i ， B a j ， B g i ， B g j ] x=[Pi，Pj，Vi，Vj，Qi，Qj，Bai，Baj，Bgi，Bgj] x=[Pi，Pj，Vi，Vj，Qi，Qj，Bai，Baj，Bgi，Bgj]
两帧之间预积分为 m = [ α i j , β i j , θ i j ] m=[\alpha_{ij}, \beta_{ij}, \theta_{ij}] m=[αij,βij,θij]
那么对于两帧之间的状态变量有
δ P i j = R w i ( P j − P i + 0.5 g t 2 − V i t ) − α i j δ V i j = R w i ( V j − V i + g t ) − β i j δ q i j = Q i j − 1 ⊗ Q i − 1 ⊗ Q j δ B a i j = B a j − B a i δ B g i j = B g j − B g i \delta P_{ij}=R_w^i(P_j-P_i+0.5gt^2-V_it)-\alpha_{ij} \\ \delta V_{ij}=R_w^i(V_j-V_i+gt)-\beta_{ij} \\ \delta q_{ij}=Q_{ij}^{-1}\otimes{Q_i^{-1}}\otimes{Q_j} \\ \delta Ba_{ij}=Baj-Bai \\ \delta Bg_{ij}=Bgj-Bgi \\ δPij=Rwi(Pj−Pi+0.5gt2−Vit)−αijδVij=Rwi(Vj−Vi+gt)−βijδqij=Qij−1⊗Qi−1⊗QjδBaij=Baj−BaiδBgij=Bgj−Bgi
可见上述的公式类似于一个位姿图的样子
残差对于状态变量的jacobian
其实我们都知道，在整个优化问题中，jacobian的求解还是比较重要的，因为它直接构成了增量方程，我们下面主要看一下关于上面残差关于状态变量的jacobian
a. 对于i时刻的位姿Pi，Qi
- δ P i j \delta P_{ij} δPij对于Pi，Qi的导数很明显为
d δ P i j d P i = − R w i d δ P i j d Q i = d δ P i j d R i w = ( R i w E x p ( δ θ ) ) T ( P j − P i + 0.5 g t 2 − V i t ) − ( R i w ) T ( P j − P i + 0.5 g t 2 − V i t ) δ θ = ( R i w δ θ × ) T ( P j − P i + 0.5 g t 2 − V i t ) = ( ( R i w ) T ( P j − P i + 0.5 g t 2 − V i t ) ) × δ θ \frac{d\delta P_{ij}}{dPi}=-R_w^i \\ \frac{d\delta P_{ij}}{dQi}=\frac{d\delta P_{ij}}{dR_i^w}=\frac{(R_i^wExp(\delta \theta))^T(P_j-P_i+0.5gt^2-V_it)-(R_i^w)^T(P_j-P_i+0.5gt^2-V_it)}{\delta \theta} = (R_i^w\delta \theta_×)^T(P_j-P_i+0.5gt^2-V_it)=((R_i^w)^T(P_j-P_i+0.5gt^2-V_it))_×\delta\theta \\ dPidδPij=−RwidQidδPij=dRiwdδPij=δθ(RiwExp(δθ))T(Pj−Pi+0.5gt2−Vit)−(Riw)T(Pj−Pi+0.5gt2−Vit)=(Riwδθ×)T(Pj−Pi+0.5gt2−Vit)=((Riw)T(Pj−Pi+0.5gt2−Vit))×δθ - δ V i j \delta V_{ij} δVij对于Pi，Qi的导数很明显为
d δ V i j d P i = − R w i d δ V i j d Q i = d δ V i j d R i w = ( R i w E x p ( δ θ ) ) T ( V j − V i + g t ) − ( R i w ) T ( V j − V i + g t ) δ θ = ( R i w δ θ × ) T ( V j − V i + g t ) = ( ( R i w ) T ( V j − V i + g t ) × δ θ \frac{d\delta V_{ij}}{dPi}=-R_w^i \\ \frac{d\delta V_{ij}}{dQi}=\frac{d\delta V_{ij}}{dR_i^w}=\frac{(R_i^wExp(\delta \theta))^T(V_j-V_i+gt)-(R_i^w)^T(V_j-V_i+gt)}{\delta \theta} = (R_i^w\delta \theta_×)^T(V_j-V_i+gt)=((R_i^w)^T(V_j-V_i+gt)_×\delta\theta dPidδVij=−RwidQidδVij=dRiwdδVij=δθ(RiwExp(δθ))T(Vj−Vi+gt)−(Riw)T(Vj−Vi+gt)=(Riwδθ×)T(Vj−Vi+gt)=((Riw)T(Vj−Vi+gt)×δθ - δ r i j \delta r_{ij} δrij对于Pi的导数很明显为0，但是对于Qi的导数，这里我们要进行一下简单的推导
设 q i j t = ( q i ⊗ [ 1 0.5 δ θ ] ) − 1 ⊗ q j , q i j = q i j q_{ijt}=(q_i\otimes{\begin{bmatrix}1\\0.5\delta \theta\end{bmatrix}})^{-1}\otimes q_j,\quad q_{ij}=q_{ij} qijt=(qi⊗[10.5δθ])−1⊗qj,qij=qij
有关系为 q i j t = q i j ⊗ δ q i j q_{ijt}=q_{ij}\otimes\delta q_{ij} qijt=qij⊗δqij
将公式代入之后就可以得到
δ q i j = q i j − 1 ⊗ ( q i ⊗ [ 1 0.5 δ θ ] ) − 1 ⊗ q j \delta q_{ij}=q_{ij}^{-1}\otimes (q_i\otimes{\begin{bmatrix}1\\0.5\delta \theta\end{bmatrix}})^{-1}\otimes q_j δqij=qij−1⊗(qi⊗[10.5δθ])−1⊗qj
对上述公式都提取一个逆之后就能得到
δ q i j = [ q j − 1 ⊗ q i ⊗ [ 1 0.5 δ θ ] ⊗ q i j ] − 1 \delta q_{ij}=[q_j^{-1}\otimes q_i \otimes \begin{bmatrix}1\\0.5\delta \theta\end{bmatrix} \otimes q_{ij}]^{-1} δqij=[qj−1⊗qi⊗[10.5δθ]⊗qij]−1
将上述的四元数符号改为矩阵相乘之后
δ q i j = [ [ q j − 1 ⊗ q i ] L [ q i j ] R [ 1 0.5 δ θ ] ] − 1 \delta q_{ij}=[[q_j^{-1}\otimes q_i]_L[q_{ij}]_R \begin{bmatrix}1\\0.5\delta \theta\end{bmatrix}]^{-1} δqij=[[qj−1⊗qi]L[qij]R[10.5δθ]]−1
将 δ q i j = [ 1 0.5 δ r i j ] \delta q_{ij}=\begin{bmatrix}1\\0.5\delta r_{ij}\end{bmatrix} δqij=[10.5δrij]代入同时因为四元数的逆的性质，可以得到下面的公式
δ q i j = [ 1 0.5 δ r i j ] = [ q j − 1 ⊗ q i ] L [ q i j ] R [ 1 − 0.5 δ θ ] \delta q_{ij}=\begin{bmatrix}1\\0.5\delta r_{ij}\end{bmatrix}=[q_j^{-1}\otimes q_i]_L[q_{ij}]_R \begin{bmatrix}1\\-0.5\delta \theta\end{bmatrix} δqij=[10.5δrij]=[qj−1⊗qi]L[qij]R[1−0.5δθ]
所以可以得到 d δ r i j d δ θ i = − [ [ q j − 1 ⊗ q i ] L [ q i j ] R ] r i g h t b o t t o m 3 ∗ 3 \frac{d\delta r_{ij}}{d\delta \theta_i}=-[[q_j^{-1}\otimes q_i]_L[q_{ij}]_R]_{right bottom3*3} dδθidδrij=−[[qj−1⊗qi]L[qij]R]rightbottom3∗3
可以看到，因为 δ q i j \delta q_{ij} δqij中不在包含Pi了，因此其他的都可以直接为0了
- δ b a \delta ba δba不包含Pi和Qi，因此就不用求解，直接为0
- δ b g \delta bg δbg不包含Pi和Qi，因此就不用求解，直接为0
b. 对于i时刻的速度和bias(Vi, bai, bgi)
- δ P i j \delta P_{ij} δPij对于Vi的导数很明显为
d δ P i j d V i = − R w i t \frac{d\delta P_{ij}}{dVi}=-R_w^it dVidδPij=−Rwit
对于ba和bg，这里注意，预积分的优势就是将i时刻(相机，GPS或其他传感器)的姿态Ri给踢了出去，这样就不用依赖i时刻的位姿而进行自己的积分，因此bg，ba和Pi，Vi，Qi和Pj，Vj，Qj一点儿关系都没有的，因此它之和预积分的值有关系，所以
d δ P i j d b a i = d α i j d b a i = − J p _ b a d δ P i j d b g i = d α i j d b g i = − J p _ b g \frac{d\delta P_{ij}}{dbai}=\frac{d\alpha_{ij}}{dbai}=-Jp\_ba \\ \frac{d\delta P_{ij}}{dbgi}=\frac{d\alpha_{ij}}{dbgi}=-Jp\_bg dbaidδPij=dbaidαij=−Jp_badbgidδPij=dbgidαij=−Jp_bg - δ V i j \delta V_{ij} δVij对于Vi，bg，ba的导数很明显为
d δ V i j d V i = − R w i d δ V i j d b a i = d α i j d b a i = − J v _ b a d δ V i j d b g i = d α i j d b g i = − J v _ b g \frac{d\delta V_{ij}}{dVi}=-R_w^i \\ \frac{d\delta V_{ij}}{dbai}=\frac{d\alpha_{ij}}{dbai}=-Jv\_ba \\ \frac{d\delta V_{ij}}{dbgi}=\frac{d\alpha_{ij}}{dbgi}=-Jv\_bg dVidδVij=−RwidbaidδVij=dbaidαij=−Jv_badbgidδVij=dbgidαij=−Jv_bg - δ q i j \delta q_{ij} δqij对于Vi，ba的导数很明显为0，但是对于bg的导数进行简单的推导
设 q i j t = q i − 1 ⊗ q j , q i j = q i j ⊗ [ 1 0.5 J q _ b g δ b g ] q_{ijt}=q_i^{-1}\otimes q_j,\quad q_{ij}=q_{ij}\otimes \begin{bmatrix}1\\0.5J_{q\_bg}\delta{bg}\end{bmatrix} qijt=qi−1⊗qj,qij=qij⊗[10.5Jq_bgδbg]
和上面的推导过程一样，我们通过化简等步骤可以得到
δ q i j = [ q j − 1 ⊗ q i ⊗ q i j ⊗ [ 1 0.5 J q _ b g δ b g ] ] − 1 \delta q_{ij}=[q_j^{-1}\otimes q_i \otimes q_{ij} \otimes \begin{bmatrix}1\\0.5J_{q\_bg}\delta{bg}\end{bmatrix}]^{-1} δqij=[qj−1⊗qi⊗qij⊗[10.5Jq_bgδbg]]−1
所以同样把四元数的运算变为矩阵运算之后，得到
δ q i j = [ 1 0.5 δ r i j ] = [ q j − 1 ⊗ q i ⊗ q i j ] L [ 1 − 0.5 J q _ b g δ b g ] \delta q_{ij}=\begin{bmatrix}1\\0.5\delta r_{ij}\end{bmatrix}=[q_j^{-1}\otimes q_i\otimes q_{ij}]_L \begin{bmatrix}1\\-0.5J_{q\_bg}\delta{bg}\end{bmatrix} δqij=[10.5δrij]=[qj−1⊗qi⊗qij]L[1−0.5Jq_bgδbg]
所以可以得到 d δ r i j d δ b g = − [ [ q j − 1 ⊗ q i ⊗ q i j ] L ] r i g h t b o t t o m 3 ∗ 3 ∗ J q _ b g \frac{d\delta r_{ij}}{d\delta{bg}}=-[[q_j^{-1}\otimes q_i \otimes q_{ij}]_L]_{right bottom3*3}*J_{q\_bg} dδbgdδrij=−[[qj−1⊗qi⊗qij]L]rightbottom3∗3∗Jq_bg
- δ b a \delta ba δba不包含Vi，因此为0，而对于Bai的导数为-I
- δ b g \delta bg δbg不包含Vi，因此为0，而对于Bgi的导数为-I
c. 对于j时刻的位姿Pj，Qj
-
δ P i j \delta P_{ij} δPij对于Qj的导数很明显为0，对Pj的导数为
d δ P i j d P j = R w i \frac{d\delta P_{ij}}{dPj}=R_w^i dPjdδPij=Rwi -
δ V i j \delta V_{ij} δVij对于Pj，Qj的导数很明显为0
-
δ q i j \delta q_{ij} δqij对于Pj为0，但是对于Qj，同样需要推导
设 q i j t = q i − 1 ⊗ q j ⊗ [ 1 0.5 δ θ j ] , q i j = q i j q_{ijt}=q_i^{-1}\otimes q_j\otimes{\begin{bmatrix}1\\0.5\delta \theta_j\end{bmatrix}},\quad q_{ij}=q_{ij} qijt=qi−1⊗qj⊗[10.5δθj],qij=qij
有关系为 q i j t = q i j ⊗ δ q i j q_{ijt}=q_{ij}\otimes\delta q_{ij} qijt=qij⊗δqij
将公式代入之后就可以得到
δ q i j = q i j − 1 ⊗ q i − 1 ⊗ q j ⊗ [ 1 0.5 δ θ j ] \delta q_{ij}=q_{ij}^{-1}\otimes q_i^{-1}\otimes q_j\otimes{\begin{bmatrix}1\\0.5\delta \theta_j\end{bmatrix}} δqij=qij−1⊗qi−1⊗qj⊗[10.5δθj]
可以看到，这时候我们根本不用取逆了
将上述的四元数符号改为矩阵相乘之后
δ q i j = [ q i j − 1 ⊗ q i − 1 ⊗ q j ] L [ 1 0.5 δ θ ] \delta q_{ij}=[q_{ij}^{-1}\otimes q_i^{-1}\otimes q_j]_L \begin{bmatrix}1\\0.5\delta \theta\end{bmatrix} δqij=[qij−1⊗qi−1⊗qj]L[10.5δθ]
所以可以得到 d δ r i j d δ θ i = [ [ q i j − 1 ⊗ q i − 1 ⊗ q j ] L ] r i g h t b o t t o m 3 ∗ 3 \frac{d\delta r_{ij}}{d\delta \theta_i}=[[q_{ij}^{-1}\otimes q_i^{-1}\otimes q_j]_L]_{right bottom3*3} dδθidδrij=[[qij−1⊗qi−1⊗qj]L]rightbottom3∗3
- δ b a \delta ba δba不包含Pj和Qj，因此就不用求解，直接为0
- δ b g \delta bg δbg不包含Pj和Qj，因此就不用求解，直接为0
d. 对于j时刻的速度和bias(Vj, baj, bgj)
- δ P i j \delta P_{ij} δPij对于Vj的导数很明显为0，对于baj而言，因为在该段时间内，预积分使用的bias均是i时刻的bias，修正的也是i时刻的bias，因此此处对于j时刻的bias的导数都是0
- δ V i j \delta V_{ij} δVij对于baj，bgj导数均为0，Vj的导数为
d δ V i j d V j = R w i \frac{d\delta V_{ij}}{dVj}=R_w^i dVjdδVij=Rwi - δ q i j \delta q_{ij} δqij对于Vj，baj，bgj的导数都是0
- δ b a \delta ba δba不包含Vj，因此为0，而对于Baj的导数为I
- δ b g \delta bg δbg不包含Vj，因此为0，而对于Bgj的导数为I
总结
整个推导过程就是这些，整体来说过程还是比较标准的～