《预积分总结与公式推导》学习笔记2

这篇博客详细探讨了IMU预积分噪声的传播,包括旋转、速度和位移噪声的递推式,并解析了偏置微小更新对预积分测量值的影响,通过一阶线性化近似处理偏置变化。同时,介绍了预积分残差及其雅克比矩阵,为理解IMU数据处理提供理论基础。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

预积分噪声传播

上一篇博客的最后介绍了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=ij1(ΔR~k+1jTJr((ω~kbkg)Δt)ηkgdΔt) δϕ⃗ij−1→δϕ⃗ij\delta \vec { \phi } _ { i j - 1 } \rightarrow \delta \vec { \phi } _ { i j }δϕ ij1δϕ 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~jj1δϕ ij1+Jr(ω~j1bj1g)ηj1gdΔt

速度噪声递推式

δvij−1→δvij\delta \mathbf { v } _ { i j - 1 } \rightarrow \delta \mathbf { v } _ { i j }δvij1δ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=δvij1+ΔR~ij1ηj1adΔtΔR~ij1(f~j1bia)δϕ ij1Δt

位移噪声递推式

δpij−1→δpij\delta \mathbf { p } _ { i j - 1 } \rightarrow \delta \mathbf { p } _ { i j }δpij1δ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

评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值