VINS预积分推导
数学基础
可以通过运算 ( ⋅ ) ∧ {\left( \cdot \right)^ \wedge } (⋅)∧将三维向量 ω ∈ R 3 \omega \in {\mathbb{R}^{3}} ω∈R3转换为对角线为零的反对称矩阵 ω ∧ ∈ R 3 × 3 \ {\omega ^ \wedge } \in {\mathbb{R}^{3 \times 3}} ω∧∈R3×3：
ω ∧ = [ ω 1 ω 2 ω 3 ] ∧ = [ 0 − ω 3 ω 2 ω 3 0 − ω 1 − ω 2 ω 1 0 ] (1.1) {\omega ^ \wedge } = {\left[ \begin{array}{l}{\omega _1}\\{\omega _2}\\{\omega _3}\end{array} \right]^ \wedge } = \left[ {\begin{array}{ccccccccccccccc}0&{ - {\omega _3}}&{{\omega _2}}\\{{\omega _3}}&0&{ - {\omega _1}}\\{ - {\omega _2}}&{{\omega _1}}&0\end{array}} \right] \tag{1.1} ω∧= ω1ω2ω3 ∧= 0ω3−ω2−ω30ω1ω2−ω10 (1.1)
同理可以使用 ( ⋅ ) ∨ {\left( \cdot \right)^ \vee } (⋅)∨运算将一个对角线为零的反对称矩阵转换为三维向量。
运算 ( ⋅ ) ∧ {\left( \cdot \right)^ \wedge } (⋅)∧满足：
a ∧ b = − b ∧ a , ∀ a , b ∈ R 3 {a^ \wedge }b = - {b^ \wedge }a,{\rm{ }}\forall a, b \in {\mathbb{R}^{3}} a∧b=−b∧a,∀a,b∈R3
对于旋转向量 ϕ ∈ s o ( 3 ) \phi \in \mathfrak{s}\mathfrak{o}\left( 3 \right) ϕ∈so(3)，可以通过指数映射 exp ( ⋅ ) \exp \left( {\cdot } \right) exp(⋅)将其转换为旋转矩阵 R ∈ S O ( 3 ) R \in SO\left( 3 \right) R∈SO(3)：
R = exp ( ϕ ∧ ) = I + sin ( ∥ ϕ ∥ ) ∥ ϕ ∥ ϕ ∧ + 1 − cos ( ∥ ϕ ∥ ) ∥ ϕ ∥ 2 ( ϕ ) 2 (1.2) R = \exp \left( {{\phi ^ \wedge }} \right) = I + \frac{{\sin \left( {\left\| \phi \right\|} \right)}}{{\left\| \phi \right\|}}{\phi ^ \wedge } + \frac{{1 - \cos \left( {\left\| \phi \right\|} \right)}}{{{{\left\| \phi \right\|}^2}}}{\left( \phi \right)^2}\tag{1.2} R=exp(ϕ∧)=I+∥ϕ∥sin(∥ϕ∥)ϕ∧+∥ϕ∥21−cos(∥ϕ∥)(ϕ)2(1.2)
同时，式 ( 1.2 ) (1.2) (1.2)可以进行一阶近似：
R = exp ( ϕ ∧ ) ≈ I + ϕ ∧ (1.3) R = \exp \left( {{\phi ^ \wedge }} \right) \approx I + {\phi ^ \wedge }\tag{1.3} R=exp(ϕ∧)≈I+ϕ∧(1.3)
同理，旋转矩阵 R R R可以通过对数映射 log ( ⋅ ) \log \left( {\cdot } \right) log(⋅)将其转换为旋转向量。
通过BCH公式，可以将李代数上的加法近似为李群上的乘法：
exp ( ϕ + δ ϕ ) ≈ exp ( ϕ ) exp ( J r ( ϕ ) δ ϕ ) (1.4) \exp \left( {\phi + \delta \phi } \right) \approx \exp \left( \phi \right)\exp \left( {{J_r}\left( \phi \right)\delta \phi } \right)\tag{1.4} exp(ϕ+δϕ)≈exp(ϕ)exp(Jr(ϕ)δϕ)(1.4) J r ( ϕ ) = I − 1 − cos ( ∥ ϕ ∥ ) ∥ ϕ ∥ 2 ϕ ∧ + ∥ ϕ ∥ − sin ( ∥ ϕ ∥ ) ∥ ϕ 3 ∥ ( ϕ ∧ ) 2 (1.5) {J_r}\left( \phi \right) = I - \frac{{1 - \cos \left( {\left\| \phi \right\|} \right)}}{{{{\left\| \phi \right\|}^2}}}{\phi ^ \wedge } + \frac{{\left\| \phi \right\| - \sin \left( {\left\| \phi \right\|} \right)}}{{\left\| {{\phi ^3}} \right\|}}{\left( {{\phi ^ \wedge }} \right)^2}\tag{1.5} Jr(ϕ)=I−∥ϕ∥21−cos(∥ϕ∥)ϕ∧+∥ϕ3∥∥ϕ∥−sin(∥ϕ∥)(ϕ∧)2(1.5)
也可以将李群上的乘法近似为李代数上的加法:
log ( exp ( ϕ ) exp ( δ ϕ ) ) ∨ ≈ ϕ + J r − 1 ( ϕ ) δ ϕ (1.6) \log {\left( {\exp \left( \phi \right)\exp \left( {\delta \phi } \right)} \right)^ \vee } \approx \phi + J_r^{ - 1}\left( \phi \right)\delta \phi\tag{1.6} log(exp(ϕ)exp(δϕ))∨≈ϕ+Jr−1(ϕ)δϕ(1.6) J r − 1 ( ϕ ) = I + 1 2 ϕ ∧ + ( 1 ∥ ϕ ∥ 2 + 1 + cos ( ∥ ϕ ∥ ) 2 ∥ ϕ ∥ sin ( ∥ ϕ ∥ ) ) ( ϕ ∧ ) 2 (1.7) J_r^{ - 1}\left( \phi \right) = I + \frac{1}{2}{\phi ^ \wedge } + \left( {\frac{1}{{{{\left\| \phi \right\|}^2}}} + \frac{{1 + \cos \left( {\left\| \phi \right\|} \right)}}{{2\left\| \phi \right\|\sin \left( {\left\| \phi \right\|} \right)}}} \right){\left( {{\phi ^ \wedge }} \right)^2}\tag{1.7} Jr−1(ϕ)=I+21ϕ∧+(∥ϕ∥21+2∥ϕ∥sin(∥ϕ∥)1+cos(∥ϕ∥))(ϕ∧)2(1.7)
李群的转置性质：
exp ( ϕ ∧ ) R = R exp ( R T ϕ ∧ ) (1.8) \exp \left( {{\phi ^ \wedge }} \right)R = { R} \exp \left( {{R^T}{\phi ^ \wedge }} \right)\tag{1.8} exp(ϕ∧)R=Rexp(RTϕ∧)(1.8)
四元数
单位四元数表达三维空间的旋转，由一个实部 s ∈ R s \in \mathbb{R} s∈R和一个虚部 v ∈ R 3 × 1 v \in {\mathbb{R}^{3 \times 1}} v∈R3×1组成：
q = [ w x y z ] = [ s v ] q = \left[ \begin{array}{l}w\\x\\y\\z\end{array} \right] = \left[ \begin{array}{l}s\\v\end{array} \right] q= wxyz =[sv]
四元数的乘法运算 ⊗ \otimes ⊗定义如下：
q a ⊗ q b = [ s a s b − v a T v b s a v b + s b v a + v a × v b ] {q_a} \otimes {q_b} = \left[ \begin{array}{l}{s_a}{s_b} - v_a^T{v_b}\\{s_a}{v_b} + {s_b}{v_a} + {v_a} \times {v_b}\end{array} \right] qa⊗qb=[sasb−vaTvbsavb+sbva+va×vb]推导过程中经常将四元数乘法转换为矩阵乘法： q a ⊗ q b = [ q a ] L q b = q a [ q b ] R {q_a} \otimes {q_b} = {\left[ {{q_a}} \right]_L}{q_b} = {q_a}{\left[ {{q_b}} \right]_R} qa⊗qb=[qa]Lqb=qa[qb]R其中： [ q a ] L = [ w − x − y − z x w − z y y z w − x z − y x w ] , [ q b ] R = [ w − x − y − z x w z − y y − z w x z y − x w ] {\left[ {{q_a}} \right]_L} = \left[ {\begin{array}{ccccccccccccccc}w&{ - x}&{ - y}&{ - z}\\x&w&{ - z}&y\\y&z&w&{ - x}\\z&{ - y}&x&w\end{array}} \right],{\left[ {{q_b}} \right]_R} = \left[ {\begin{array}{ccccccccccccccc}w&{ - x}&{ - y}&{ - z}\\x&w&z&{ - y}\\y&{ - z}&w&x\\z&y&{ - x}&w\end{array}} \right] [qa]L= wxyz−xwz−y−y−zwx−zy−xw ,[qb]R= wxyz−xw−zy−yzw−x−z−yxw 同时，四元数和旋转向量可以相互转换： q = [ cos θ 2 n ⃗ sin θ 2 ] q = \left[ \begin{array}{l}\cos \frac{\theta }{2}\\\vec n\sin \frac{\theta }{2}\end{array} \right] q=[cos2θnsin2θ]当 θ \theta θ为小量时，有以下近似： q = [ 1 θ n ⃗ 2 ] q = \left[ \begin{array}{c}1\\\frac{{\theta \vec n}}{2}\end{array} \right] q=[12θn]
通常使用右乘扰动模型对李群求导，左乘和右乘等价，只不过扰动模型不能混用。李群可以使用旋转矩阵也可以使用四元数。
∂ ( R p ) ∂ ϕ = lim δ ϕ → 0 exp ( δ ϕ ∧ ) exp ( ϕ ∧ ) p − exp ( ϕ ∧ ) p δ ϕ = lim δ ϕ → 0 ( I + δ ϕ ∧ ) exp ( ϕ ∧ ) p − exp ( ϕ ∧ ) p δ ϕ = lim δ ϕ → 0 δ ϕ ∧ exp ( ϕ ∧ ) p δ ϕ = lim δ ϕ → 0 − ( exp ( ϕ ∧ ) p ) ∧ δ ϕ δ ϕ = − ( exp ( ϕ ∧ ) p ) ∧ \begin{array}{l}\frac{{\partial \left( {Rp} \right)}}{{\partial \phi }} = \lim_{\delta \phi \to 0} \frac{{\exp \left( {\delta {\phi ^ \wedge }} \right)\exp \left( {{\phi ^ \wedge }} \right)p - \exp \left( {{\phi ^ \wedge }} \right)p}}{{\delta \phi }}\\ = \lim_{\delta \phi \to 0} \frac{{\left( {I + \delta {\phi ^ \wedge }} \right)\exp \left( {{\phi ^ \wedge }} \right)p - \exp \left( {{\phi ^ \wedge }} \right)p}}{{\delta \phi }}\\ = \lim_{\delta \phi \to 0} \frac{{\delta {\phi ^ \wedge }\exp \left( {{\phi ^ \wedge }} \right)p}}{{\delta \phi }}\\ = \lim_{\delta \phi \to 0} \frac{{ - {{\left( {\exp \left( {{\phi ^ \wedge }} \right)p} \right)}^ \wedge }\delta \phi }}{{\delta \phi }}\\ = - {\left( {\exp \left( {{\phi ^ \wedge }} \right)p} \right)^ \wedge }\end{array} ∂ϕ∂(Rp)=limδϕ→0δϕexp(δϕ∧)exp(ϕ∧)p−exp(ϕ∧)p=limδϕ→0δϕ(I+δϕ∧)exp(ϕ∧)p−exp(ϕ∧)p=limδϕ→0δϕδϕ∧exp(ϕ∧)p=limδϕ→0δϕ−(exp(ϕ∧)p)∧δϕ=−(exp(ϕ∧)p)∧
∂ ( R p ) ∂ ϕ = lim δ ϕ → 0 exp ( ϕ ∧ ) exp ( δ ϕ ∧ ) p − exp ( ϕ ∧ ) p δ ϕ = lim δ ϕ → 0 exp ( ϕ ∧ ) ( I + δ ϕ ∧ ) p − exp ( ϕ ∧ ) p δ ϕ = lim δ ϕ → 0 exp ( ϕ ∧ ) δ ϕ ∧ p δ ϕ = lim δ ϕ → 0 − exp ( ϕ ∧ ) p ∧ δ ϕ δ ϕ = − exp ( ϕ ∧ ) p ∧ \begin{array}{l}\frac{{\partial \left( {Rp} \right)}}{{\partial \phi }} = \lim_{\delta \phi \to 0} \frac{{\exp \left( {{\phi ^ \wedge }} \right)\exp \left( {\delta {\phi ^ \wedge }} \right)p - \exp \left( {{\phi ^ \wedge }} \right)p}}{{\delta \phi }}\\{\rm{ = }} \lim_{\delta \phi \to 0} \frac{{\exp \left( {{\phi ^ \wedge }} \right)\left( {I + \delta {\phi ^ \wedge }} \right)p - \exp \left( {{\phi ^ \wedge }} \right)p}}{{\delta \phi }}\\{\rm{ = }} \lim_{\delta \phi \to 0} \frac{{\exp \left( {{\phi ^ \wedge }} \right)\delta {\phi ^ \wedge }p}}{{\delta \phi }}\\{\rm{ = }} \lim_{\delta \phi \to 0} \frac{{ - \exp \left( {{\phi ^ \wedge }} \right){p^ \wedge }\delta \phi }}{{\delta \phi }}\\ = - \exp \left( {{\phi ^ \wedge }} \right){p^ \wedge }\end{array} ∂ϕ∂(Rp)=limδϕ→0δϕexp(ϕ∧)exp(δϕ∧)p−exp(ϕ∧)p=limδϕ→0δϕexp(ϕ∧)(I+δϕ∧)p−exp(ϕ∧)p=limδϕ→0δϕexp(ϕ∧)δϕ∧p=limδϕ→0δϕ−exp(ϕ∧)p∧δϕ=−exp(ϕ∧)p∧ ∂ log ( R 1 R 2 ) ∂ ϕ 2 = lim δ ϕ 2 → 0 log ( R 1 R 2 exp ( δ ϕ 2 ∧ ) ) − log ( R 1 R 2 ) δ ϕ 2 = lim δ ϕ 2 → 0 log ( R 1 R 2 ) + J r − 1 ( log ( R 1 R 2 ) ) δ ϕ 2 − log ( R 1 R 2 ) δ ϕ 2 = J r − 1 ( log ( R 1 R 2 ) ) \begin{array}{l}\frac{{\partial \log \left( {{R_1}{R_2}} \right)}}{{\partial {\phi _2}}} = \lim_{\delta {\phi _2} \to 0} \frac{{\log \left( {{R_1}{R_2}\exp \left( {\delta \phi _2^ \wedge } \right)} \right) - \log \left( {{R_1}{R_2}} \right)}}{{\delta {\phi _2}}}\\ = \lim_{\delta {\phi _2} \to 0} \frac{{\log \left( {{R_1}{R_2}} \right) + J_r^{ - 1}\left( {\log \left( {{R_1}{R_2}} \right)} \right)\delta {\phi _2} - \log \left( {{R_1}{R_2}} \right)}}{{\delta {\phi _2}}}\\ = J_r^{ - 1}\left( {\log \left( {{R_1}{R_2}} \right)} \right)\end{array} ∂ϕ2∂log(R1R2)=limδϕ2→0δϕ2log(R1R2exp(δϕ2∧))−log(R1R2)=limδϕ2→0δϕ2log(R1R2)+Jr−1(log(R1R2))δϕ2−log(R1R2)=Jr−1(log(R1R2)) ∂ ( R − 1 p ) ∂ ϕ = lim lim δ ϕ → 0 ( R exp ( δ ϕ ∧ ) ) − 1 p − R − 1 p δ ϕ = lim δ ϕ → 0 exp ( − δ ϕ ∧ ) R − 1 p − R − 1 p δ ϕ = lim δ ϕ → 0 ( I − δ ϕ ∧ ) R − 1 p − R − 1 p δ ϕ = lim δ ϕ → 0 − δ ϕ ∧ R − 1 p δ ϕ = lim δ ϕ → 0 ( R − 1 p ) ∧ δ ϕ δ ϕ = ( R − 1 p ) ∧ \begin{array}{l}\frac{{\partial \left( {{R^{ - 1}}p} \right)}}{{\partial \phi }} = {\lim }\lim_{\delta \phi \to 0} \frac{{{{\left( {R\exp \left( {\delta {\phi ^ \wedge }} \right)} \right)}^{ - 1}}p - {R^{ - 1}}p}}{{\delta \phi }}\\ = \lim_{\delta \phi \to 0} \frac{{\exp \left( { - \delta {\phi ^ \wedge }} \right){R^{ - 1}}p - {R^{ - 1}}p}}{{\delta \phi }}\\ = \lim_{\delta \phi \to 0} \frac{{\left( {I - \delta {\phi ^ \wedge }} \right){R^{ - 1}}p - {R^{ - 1}}p}}{{\delta \phi }}\\ = \lim_{\delta \phi \to 0} \frac{{ - \delta {\phi ^ \wedge }{R^{ - 1}}p}}{{\delta \phi }}\\ = \lim_{\delta \phi \to 0} \frac{{{{\left( {{R^{ - 1}}p} \right)}^ \wedge }\delta \phi }}{{\delta \phi }}\\ = {\left( {{R^{ - 1}}p} \right)^ \wedge }\end{array} ∂ϕ∂(R−1p)=limlimδϕ→0δϕ(Rexp(δϕ∧))−1p−R−1p=limδϕ→0δϕexp(−δϕ∧)R−1p−R−1p=limδϕ→0δϕ(I−δϕ∧)R−1p−R−1p=limδϕ→0δϕ−δϕ∧R−1p=limδϕ→0δϕ(R−1p)∧δϕ=(R−1p)∧ ∂ log ( R 1 R 2 − 1 ) ∂ ϕ 2 = lim δ ϕ 2 → 0 log ( R 1 exp ( − δ ϕ 2 ∧ ) R 2 − 1 ) − log ( R 1 R 2 ) δ ϕ 2 = lim δ ϕ 2 → 0 log ( R 1 R 2 − 1 exp ( − R 2 δ ϕ 2 ∧ ) ) − log ( R 1 R 2 ) δ ϕ 2 = lim δ ϕ 2 → 0 − J r − 1 ( log ( R 1 R 2 − 1 ) ) R 2 δ ϕ 2 δ ϕ 2 = − J r − 1 ( log ( R 1 R 2 − 1 ) ) R 2 \begin{array}{l}\frac{{\partial \log \left( {{R_1}R_2^{ - 1}} \right)}}{{\partial {\phi _2}}} = {\lim }_{\delta {\phi _2} \to 0} \frac{{\log \left( {{R_1}\exp \left( { - \delta \phi _2^ \wedge } \right)R_2^{ - 1}} \right) - \log \left( {{R_1}{R_2}} \right)}}{{\delta {\phi _2}}}\\ = {\lim }_{\delta {\phi _2} \to 0} \frac{{\log \left( {{R_1}R_2^{ - 1}\exp \left( { - {R_2}\delta \phi _2^ \wedge } \right)} \right) - \log \left( {{R_1}{R_2}} \right)}}{{\delta {\phi _2}}}\\ = {\lim }_{\delta {\phi _2} \to 0} \frac{{ - J_r^{ - 1}\left( {\log \left( {{R_1}R_2^{ - 1}} \right)} \right){R_2}\delta {\phi _2}}}{{\delta {\phi _2}}}\\ = - J_r^{ - 1}\left( {\log \left( {{R_1}R_2^{ - 1}} \right)} \right){R_2}\end{array} ∂ϕ2∂log(R1R2−1)=limδϕ2→0δϕ2log(R1exp(−δϕ2∧)R2−1)−log(R1R2)=limδϕ2→0δϕ2log(R1R2−1exp(−R2δϕ2∧))−log(R1R2)=limδϕ2→0δϕ2−Jr−1(log(R1R2−1))R2δϕ2=−Jr−1(log(R1R2−1))R2李群更新可以使用旋转矩阵也可以使用四元数，两种方法是等价的，注意四元数需要归一化： R ← R exp ( ϕ ∧ ) q ← q ⊗ [ 1 1 2 ϕ ] \begin{array}{l}R \leftarrow {{\rm R}} \exp \left( {{\phi ^ \wedge }} \right)\\q \leftarrow q \otimes \left[ \begin{array}{c}1\\\frac{1}{2}\phi \end{array} \right]\end{array} R←Rexp(ϕ∧)q←q⊗[121ϕ]
IMU
IMU的误差可以分为确定性误差和随机误差。
确定性误差主要包括比例因子误差、零偏误差、非正交误差，同时误差受到温度的影响，可以通过标定方法确定误差的大小，例如六面法标定法，温度标定法等。
随机误差通常包括零偏随机游走和高斯白噪声。高斯白噪声假设IMU数据在连续时间上受到一个均值为0，方差为 σ 2 {\sigma ^2} σ2，各时刻之间相互独立的高斯过程，对于离散的IMU数据，方差之间存在以下转化关系： σ d 2 = σ 2 Δ t \sigma _d^2 = \frac{{{\sigma ^2}}}{{\Delta t}} σd2=Δtσ2