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

预积分噪声传播

上一篇博客的最后介绍了IMU预积分可以分解为预积分测量值减去预积分噪声值的形式,但是没有给出预积分噪声的传播方式(递推关系)。本节从这一点出发,首先定义预积分噪声为:
η i j Δ ≜ [ δ ϕ ⃗ i j T δ v i j T δ p i j T ] 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其中, η i j Δ ∼ N ( 0 9 × 1 , Σ i j ) \boldsymbol { \eta } _ { i j } ^ { \Delta } \sim N \left( \mathbf { 0 } _ { 9 \times 1 } , \mathbf { \Sigma } _ { i j } \right) ηijΔN(09×1,Σij)。下面分别对三个噪声项进行分析。

旋转噪声递推式

由于噪声属于小量,所以指数映射的乘积等于指数求和后的指数映射,即 Exp ⁡ ( δ x 1 ) Exp ⁡ ( δ x 2 ) ⋯ Exp ⁡ ( δ x n ) = Exp ⁡ ( δ x 1 + δ x 1 + ⋯ + δ x n ) \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),所以
δ ϕ ⃗ i j = ∑ k = i j − 1 ( Δ R ~ k + 1 j T ⋅ J r ( ( ω ~ k − b k g ) Δ t ) ⋅ η k g d Δ 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) δ ϕ ⃗ i j − 1 → δ ϕ ⃗ i j \delta \vec { \phi } _ { i j - 1 } \rightarrow \delta \vec { \phi } _ { i j } δϕ ij1δϕ ij
δ ϕ ⃗ i j = Δ R ~ j j − 1 δ ϕ ⃗ i j − 1 + J r ( ω ~ j − 1 − b j − 1 g ) η j − 1 g d Δ 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

速度噪声递推式

δ v i j − 1 → δ v i j \delta \mathbf { v } _ { i j - 1 } \rightarrow \delta \mathbf { v } _ { i j } δvij1δvij
δ v i j = δ v i j − 1 + Δ R ~ i j − 1 η j − 1 a d Δ t − Δ R ~ i j − 1 ⋅ ( f ~ j − 1 − b i a ) ∧ ⋅ δ ϕ ⃗ i j − 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

位移噪声递推式

δ p i j − 1 → δ p i j \delta \mathbf { p } _ { i j - 1 } \rightarrow \delta \mathbf { p } _ { i j } δpij1δpij
δ p i j = δ p i j − 1 + δ v i j − 1 Δ

  • 1
    点赞
  • 3
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值