预积分噪声传播
上一篇博客的最后介绍了IMU预积分可以分解为预积分测量值减去预积分噪声值的形式,但是没有给出预积分噪声的传播方式(递推关系)。本节从这一点出发,首先定义预积分噪声为:
ηijΔ≜[δϕ⃗ijTδvijTδpijT]T \boldsymbol { \eta } _ { i j } ^ { \Delta } \triangleq \left[ \begin{array} { c c c } { \delta \vec { \phi } _ { i j } ^ { T } } & { \delta \mathbf { v } _ { i j } ^ { T } } & { \delta \mathbf { p } _ { i j } ^ { T } } \end{array} \right] ^ { T } ηijΔ≜[δϕijTδvijTδpijT]T其中,ηijΔ∼N(09×1,Σij)\boldsymbol { \eta } _ { i j } ^ { \Delta } \sim N \left( \mathbf { 0 } _ { 9 \times 1 } , \mathbf { \Sigma } _ { i j } \right)ηijΔ∼N(09×1,Σij)。下面分别对三个噪声项进行分析。
旋转噪声递推式
由于噪声属于小量,所以指数映射的乘积等于指数求和后的指数映射,即Exp(δx1)Exp(δx2)⋯Exp(δxn)=Exp(δx1+δx1+⋯+δxn)\operatorname{Exp}(\delta x_1)\operatorname{Exp}(\delta x_2)\cdots\operatorname{Exp}(\delta x_n)=\operatorname{Exp}(\delta x_1+\delta x_1+\cdots+\delta x_n)Exp(δx1)Exp(δx2)⋯Exp(δxn)=Exp(δx1+δx1+⋯+δxn),所以
δϕ⃗ij=∑k=ij−1(ΔR~k+1jT⋅Jr((ω~k−bkg)Δt)⋅ηkgdΔt) \delta \vec { \phi } _ { i j } = \sum _ { k = i } ^ { j - 1 } \left( \Delta \tilde { \mathbf { R } } _ { k + 1 j } ^ { T } \cdot \mathbf { J } _ { r } \left( \left( \tilde { \boldsymbol { \omega } } _ { k } - \mathbf { b } _ { k } ^ { g } \right) \Delta t \right) \cdot \boldsymbol { \eta } _ { k } ^ { g d } \Delta t \right) δϕij=k=i∑j−1(ΔR~k+1jT⋅Jr((ω~k−bkg)Δt)⋅ηkgdΔt) δϕ⃗ij−1→δϕ⃗ij\delta \vec { \phi } _ { i j - 1 } \rightarrow \delta \vec { \phi } _ { i j }δϕij−1→δϕij:
δϕ⃗ij=ΔR~jj−1δϕ⃗ij−1+Jr(ω~j−1−bj−1g)ηj−1gdΔt \delta \vec { \phi } _ { i j } =\Delta \tilde { \mathbf { R } } _ { j j - 1 } \delta \vec { \phi } _ { i j - 1 } + \mathbf { J } _ { r } \left( \tilde { \boldsymbol { \omega } } _ { j-1 } - \mathbf { b } _ { j-1 } ^ { g } \right) \mathbf { \eta } _ { j - 1 } ^ { g d } \Delta t δϕij=ΔR~jj−1δϕij−1+Jr(ω~j−1−bj−1g)ηj−1gdΔt
速度噪声递推式
δvij−1→δvij\delta \mathbf { v } _ { i j - 1 } \rightarrow \delta \mathbf { v } _ { i j }δvij−1→δvij:
δvij=δvij−1+ΔR~ij−1ηj−1adΔt−ΔR~ij−1⋅(f~j−1−bia)∧⋅δϕ⃗ij−1⋅Δt \delta \mathbf { v } _ { i j } =\delta \mathbf { v } _ { i j - 1 } + \Delta \tilde { \mathbf { R } } _ { i j - 1 } \mathbf { \eta } _ { j - 1 } ^ { a d } \Delta t - \Delta \tilde { \mathbf { R } } _ { i j - 1 } \cdot \left( \tilde { \mathbf { f } } _ { j - 1 } - \mathbf { b } _ { i } ^ { a } \right) ^ { \wedge } \cdot \delta \vec { \phi } _ { i j - 1 } \cdot \Delta t δvij=δvij−1+ΔR~ij−1ηj−1adΔt−ΔR~ij−1⋅(f~j−1−bia)∧⋅δϕij−1⋅Δt
位移噪声递推式
δpij−1→δpij\delta \mathbf { p } _ { i j - 1 } \rightarrow \delta \mathbf { p } _ { i j }δpij−1→δpij:
δpij=δpij−1+δvij−1Δt−12ΔR~ij−1⋅(f~j−1−bia)∧δϕ⃗ij−1Δt2+12ΔR~ij−1ηj−1adΔt2 \delta \mathbf { p } _ { i j } =\delta \mathbf { p } _ { i j - 1 } + \delta \mathbf { v } _ { i j - 1 } \Delta t - \frac { 1 } { 2 } \Delta \tilde { \mathbf { R } } _ { i j - 1 } \cdot \left( \tilde { \mathbf { f } } _ { j - 1 } - \mathbf { b } _ { i } ^ { a } \right) ^ { \wedge } \delta \vec { \phi } _ { i j - 1 } \Delta t ^ { 2 } + \frac { 1 } { 2 } \Delta \tilde { \mathbf { R } } _ { i j - 1 } \mathbf { \eta } _ { j - 1 } ^ { a d } \Delta t ^ { 2 } δp