> 文章列表 > ORB_SLAM3_IMU预积分理论推导(噪声分析)

ORB_SLAM3_IMU预积分理论推导(噪声分析)

ORB_SLAM3_IMU预积分理论推导(噪声分析)

噪声分析

  1. δϕ⃗ij\\delta \\vec{\\phi}_{i j}δϕij
    Exp⁡(−δϕ⃗ij)=∏k=ij−1Exp⁡(−ΔR~k+1jT⋅Jrk⋅ηkgdΔt)\\operatorname{Exp}\\left(-\\delta \\vec{\\phi}_{i j}\\right)=\\prod_{k=i}^{j-1} \\operatorname{Exp}\\left(-\\Delta \\tilde{\\mathbf{R}}_{k+1 j}^{T} \\cdot \\mathbf{J}_{r}^{k} \\cdot \\boldsymbol{\\eta}_{k}^{g d} \\Delta t\\right) Exp(δϕij)=k=ij1Exp(ΔR~k+1jTJrkηkgdΔt)
    其中,Jrk=Jr((ω~k−big)Δt)\\mathbf{J}_{r}^{k}=\\mathbf{J}_{r}\\left(\\left(\\tilde{\\boldsymbol{\\omega}}_{k}-\\mathbf{b}_{i}^{g}\\right) \\Delta t\\right)Jrk=Jr((ω~kbig)Δt)

对上式两边取对数:
δϕ⃗ij=−log⁡(∏k=ij−1Exp⁡(−ΔR~k+1jT⋅Jrk⋅ηkgdΔt))\\delta \\vec{\\phi}_{i j}=-\\log \\left(\\prod_{k=i}^{j-1} \\operatorname{Exp}\\left(-\\Delta \\tilde{\\mathbf{R}}_{k+1 j}^{T} \\cdot \\mathbf{J}_{r}^{k} \\cdot \\boldsymbol{\\eta}_{k}^{g d} \\Delta t\\right)\\right) δϕij=log(k=ij1Exp(ΔR~k+1jTJrkηkgdΔt))
利用性质:当δϕ⃗\\delta \\vec{\\phi}δϕ是小量时,log⁡(Exp⁡(ϕ⃗)⋅Exp⁡(δϕ⃗))=ϕ⃗+J⁡r−1(ϕ⃗)⋅δϕ⃗\\log{\\left(\\operatorname{Exp}\\left(\\vec{\\phi} \\right) \\cdot \\operatorname{Exp}\\left(\\delta\\vec{\\phi}\\right)\\right)}=\\vec{\\phi }+\\operatorname{J}^{-1}_{r}\\left (\\vec{\\phi } \\right ) \\cdot \\delta \\vec{\\phi }log(Exp(ϕ)Exp(δϕ))=ϕ+Jr1(ϕ)δϕ, 其中ηkgd\\boldsymbol{\\eta}_{k}^{g d}ηkgd是小量,ξ=ΔR~k+1jT⋅Jrk⋅ηkgdΔt\\xi =\\Delta \\tilde{\\mathbf{R}}_{k+1 j}^{T} \\cdot \\mathbf{J}_{r}^{k} \\cdot \\boldsymbol{\\eta}_{k}^{g d} \\Delta tξ=ΔR~k+1jTJrkηkgdΔt是小量,于是J⁡r−1(ξk)≈I\\operatorname{J}^{-1}_{r}\\left (\\xi_{k} \\right )\\approx IJr1(ξk)I

δϕ⃗ij=−log⁡(∏k=ij−1Exp⁡(−ξk))=−log⁡(Exp⁡(−ξi)∏k=i+1j−1Exp⁡(−ξk))≈−(−ξi+I⋅log⁡(∏k=i+1j−1Exp⁡(−ξk)))=ξi−log⁡(∏k=i+1j−1Exp⁡(−ξk))=ξi−log⁡(Exp⁡(−ξi+1)∏k=i+2j−1Exp⁡(−ξk))≈ξi+ξi+1−log⁡(∏k=i+2j−1Exp⁡(−ξk))≈⋯≈∑k=ij−1ξk\\begin{aligned} \\delta \\vec{\\phi}_{i j} & =-\\log \\left(\\prod_{k=i}^{j-1} \\operatorname{Exp}\\left(-\\xi_{k}\\right)\\right) \\\\ & =-\\log \\left(\\operatorname{Exp}\\left(-\\xi_{i}\\right) \\prod_{k=i+1}^{j-1} \\operatorname{Exp}\\left(-\\xi_{k}\\right)\\right) \\\\ & \\approx-\\left(-\\xi_{i}+\\mathbf{I} \\cdot \\log \\left(\\prod_{k=i+1}^{j-1} \\operatorname{Exp}\\left(-\\xi_{k}\\right)\\right)\\right)=\\xi_{i}-\\log \\left(\\prod_{k=i+1}^{j-1} \\operatorname{Exp}\\left(-\\xi_{k}\\right)\\right) \\\\ & =\\xi_{i}-\\log \\left(\\operatorname{Exp}\\left(-\\xi_{i+1}\\right) \\prod_{k=i+2}^{j-1} \\operatorname{Exp}\\left(-\\xi_{k}\\right)\\right) \\\\ & \\approx \\xi_{i}+\\xi_{i+1}-\\log \\left(\\prod_{k=i+2}^{j-1} \\operatorname{Exp}\\left(-\\xi_{k}\\right)\\right) \\\\ & \\approx \\cdots \\\\ & \\approx \\sum_{k=i}^{j-1} \\xi_{k} \\end{aligned} δϕij=log(k=ij1Exp(ξk))=log(Exp(ξi)k=i+1j1Exp(ξk))(ξi+Ilog(k=i+1j1Exp(ξk)))=ξilog(k=i+1j1Exp(ξk))=ξilog(Exp(ξi+1)k=i+2j1Exp(ξk))ξi+ξi+1log(k=i+2j1Exp(ξk))k=ij1ξk
即:
δϕ⃗ij≈∑k=ij−1ΔR~k+1jTJ⁡rkηkgdΔt\\delta\\vec{\\phi}_{ij}\\approx \\sum_{k=i}^{j-1} {\\Delta\\tilde{R}_{k+1 j}^{T}\\operatorname{J}_{r}^{k}\\eta_{k}^{gd}\\Delta t } δϕijk=ij1ΔR~k+1jTJrkηkgdΔt
由于ΔR~k+1jT\\Delta\\tilde{R}_{k+1 j}^{T}ΔR~k+1jTJ⁡rk\\operatorname{J}_{r}^{k}JrkΔt\\Delta tΔt都是已知量,而ηkgd\\eta_{k}^{gd}ηkgd为零均值高斯噪声,因此δϕ⃗ij\\delta \\vec{\\phi}_{i j}δϕij也为零均值高斯噪声
2. δvij\\delta v_{ij}δvij
δvij=∑k=ij−1[ΔR~ikηkadΔt−ΔR~ik⋅(f~k−bia)∧⋅δϕ⃗ik⋅Δt]\\delta \\mathbf{v}_{i j}=\\sum_{k=i}^{j-1}\\left[\\Delta \\tilde{\\mathbf{R}}_{i k} \\mathbf{\\eta}_{k}^{a d} \\Delta t-\\Delta \\tilde{\\mathbf{R}}_{i k} \\cdot\\left(\\tilde{\\mathbf{f}}_{k}-\\mathbf{b}_{i}^{a}\\right)^{\\wedge} \\cdot \\delta \\vec{\\phi}_{i k} \\cdot \\Delta t\\right] δvij=k=ij1[ΔR~ikηkadΔtΔR~ik(f~kbia)δϕikΔt]
3. δpij\\delta p_{ij}δpij
δpij=∑k=ij−1[δvikΔt−12ΔR~ik⋅(f~k−bia)∧δϕ⃗ikΔt2+12ΔR~ikηkadΔt2]\\delta \\mathbf{p}_{i j}=\\sum_{k=i}^{j-1}\\left[\\delta \\mathbf{v}_{i k} \\Delta t-\\frac{1}{2} \\Delta \\tilde{\\mathbf{R}}_{i k} \\cdot\\left(\\tilde{\\mathbf{f}}_{k}-\\mathbf{b}_{i}^{a}\\right)^{\\wedge} \\delta \\vec{\\phi}_{i k} \\Delta t^{2}+\\frac{1}{2} \\Delta \\tilde{\\mathbf{R}}_{i k} \\mathbf{\\eta}_{k}^{a d} \\Delta t^{2}\\right] δpij=k=ij1[δvikΔt21ΔR~ik(f~kbia)δϕikΔt2+21ΔR~ikηkadΔt2]

噪声更新

  1. δϕ→ij−1→δϕ→ij\\delta\\overrightarrow{\\phi}_{ij-1}\\to \\delta\\overrightarrow{\\phi}_{ij}δϕij1δϕij
    δϕ⃗ij=∑k=ij−1ΔR~k+1jTJrkηkgdΔt=∑k=ij−2ΔR~k+1jTJrkηkgdΔt+ΔR~jjT⏟IJrj−1ηj−1gdΔt=∑j−2k=i(ΔR~k+1j−1ΔR~j−1j)TJrkηkgdΔt+Jrj−1ηj−1gdΔt=ΔR~jj−1∑k=ij−2ΔR~k+1j−1TJrkηkgdΔt+Jrj−1ηj−1gdΔt=ΔR~jj−1δϕ⃗ij−1+Jrj−1ηj−1gdΔt\\begin{aligned} \\delta \\vec{\\phi}_{i j} & =\\sum_{k=i}^{j-1} \\Delta \\tilde{\\mathbf{R}}_{k+1 j}^{T} \\mathbf{J}_{r}^{k} \\boldsymbol{\\eta}_{k}^{g d} \\Delta t \\\\ & =\\sum_{k=i}^{j-2} {\\color{Red} \\Delta \\tilde{\\mathbf{R}}_{k+1 j}^{T}} \\mathbf{J}_{r}^{k} \\boldsymbol{\\eta}_{k}^{g d} \\Delta t+\\underbrace{\\Delta \\tilde{\\mathbf{R}}_{j j}^{T}}_{I} \\mathbf{J}_{r}^{j-1} \\boldsymbol{\\eta}_{j-1}^{g d} \\Delta t \\\\ &= \\sum_{j-2}^{k=i}{\\color{Red} \\left(\\Delta \\tilde{\\mathbf{R}}_{k+1 j-1} \\Delta \\tilde{\\mathbf{R}}_{j-1 j}\\right)^{T}} \\mathbf{J}_{r}^{k} \\boldsymbol{\\eta}_{k}^{g d} \\Delta t+\\mathbf{J}_{r}^{j-1} \\boldsymbol{\\eta}_{j-1}^{g d} \\Delta t \\\\ & =\\Delta \\tilde{\\mathbf{R}}_{j j-1} \\sum_{k=i}^{j-2} \\Delta \\tilde{\\mathbf{R}}_{k+1 j-1}^{T} \\mathbf{J}_{r}^{k} \\boldsymbol{\\eta}_{k}^{g d} \\Delta t+\\mathbf{J}_{r}^{j-1} \\boldsymbol{\\eta}_{j-1}^{g d} \\Delta t \\\\ & =\\Delta \\tilde{\\mathbf{R}}_{j j-1} \\delta \\vec{\\phi}_{i j-1}+\\mathbf{J}_{r}^{j-1} \\boldsymbol{\\eta}_{j-1}^{g d} \\Delta t \\end{aligned} δϕij=k=ij1ΔR~k+1jTJrkηkgdΔt=k=ij2ΔR~k+1jTJrkηkgdΔt+IΔR~jjTJrj1ηj1gdΔt=j2k=i(ΔR~k+1j1ΔR~j1j)TJrkηkgdΔt+Jrj1ηj1gdΔt=ΔR~jj1k=ij2ΔR~k+1j1TJrkηkgdΔt+Jrj1ηj1gdΔt=ΔR~jj1δϕij1+Jrj1ηj1gdΔt
  2. δvij−1→δvij\\delta v_{ij-1} \\to \\delta v_{ij}δvij1δvij
    δvij=∑k=ij−1[ΔR~ikηkadΔt−ΔR~ik⋅(f~k−bia)∧⋅δϕ⃗ik⋅Δt]=∑k=ij−2[ΔR~ikηkadΔt−ΔR~ik⋅(f~k−bia)∧⋅δϕ⃗ik⋅Δt]…+ΔR~ij−1ηj−1adΔt−ΔR~ij−1⋅(f~j−1−bia)∧⋅δϕ⃗ij−1⋅Δt=δvij−1+ΔR~ij−1ηj−1adΔt−ΔR~ij−1⋅(f~j−1−bia)∧⋅δϕ⃗ij−1⋅Δt\\begin{aligned} \\delta \\mathbf{v}_{i j}= & \\sum_{k=i}^{j-1}\\left[\\Delta \\tilde{\\mathbf{R}}_{i k} \\mathbf{\\eta}_{k}^{a d} \\Delta t-\\Delta \\tilde{\\mathbf{R}}_{i k} \\cdot\\left(\\tilde{\\mathbf{f}}_{k}-\\mathbf{b}_{i}^{a}\\right)^{\\wedge} \\cdot \\delta \\vec{\\phi}_{i k} \\cdot \\Delta t\\right] \\\\ = & \\sum_{k=i}^{j-2}\\left[\\Delta \\tilde{\\mathbf{R}}_{i k} \\boldsymbol{\\eta}_{k}^{a d} \\Delta t-\\Delta \\tilde{\\mathbf{R}}_{i k} \\cdot\\left(\\tilde{\\mathbf{f}}_{k}-\\mathbf{b}_{i}^{a}\\right)^{\\wedge} \\cdot \\delta \\vec{\\phi}_{i k} \\cdot \\Delta t\\right] \\ldots \\\\ & +\\Delta \\tilde{\\mathbf{R}}_{i j-1} \\boldsymbol{\\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 \\\\ = & \\delta \\mathbf{v}_{i j-1}+\\Delta \\tilde{\\mathbf{R}}_{i j-1} \\boldsymbol{\\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 \\end{aligned} δvij===k=ij1[ΔR~ikηkadΔtΔR~ik(f~kbia)δϕikΔt]k=ij2[ΔR~ikηkadΔtΔR~ik(f~kbia)δϕikΔt]+ΔR~ij1ηj1adΔtΔR~ij1(f~j1bia)δϕij1Δtδvij1+ΔR~ij1ηj1adΔtΔR~ij1(f~j1bia)δϕij1Δt
  3. δpij−1→δpij\\delta p_{ij-1} \\to \\delta p_{ij}δpij1δpij
    δpij=∑k=ij−1[δvikΔt−12ΔR~ik⋅(f~k−bia)∧δϕ⃗ikΔt2+12ΔR~ikηkadΔt2]=δpij−1+δvij−1Δt−12ΔR~ij−1⋅(f~j−1−bia)∧δϕ⃗ij−1Δt2+12ΔR~ij−1ηj−1adΔt2\\begin{aligned} \\delta \\mathbf{p}_{i j} & =\\sum_{k=i}^{j-1}\\left[\\delta \\mathbf{v}_{i k} \\Delta t-\\frac{1}{2} \\Delta \\tilde{\\mathbf{R}}_{i k} \\cdot\\left(\\tilde{\\mathbf{f}}_{k}-\\mathbf{b}_{i}^{a}\\right)^{\\wedge} \\delta \\vec{\\phi}_{i k} \\Delta t^{2}+\\frac{1}{2} \\Delta \\tilde{\\mathbf{R}}_{i k} \\boldsymbol{\\eta}_{k}^{a d} \\Delta t^{2}\\right] \\\\ & =\\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} \\boldsymbol{\\eta}_{j-1}^{a d} \\Delta t^{2} \\end{aligned} δpij=k=ij1[δvikΔt21ΔR~ik(f~kbia)δϕikΔt2+21ΔR~ikηkadΔt2]=δpij1+δvij1Δt21ΔR~ij1(f~j1bia)δϕij1Δt2+21ΔR~ij1ηj1adΔt2
    矩阵C:
    χij=[ηijT,γijT]T∼N(015×1,Cij)ηij=[δϕijT,δvijT,δpijT]T∼N(09×1,Σijη)γij=[δbijaT,δbijgT]T∼N(06×1,Σijγ)\\begin{aligned} \\chi_{i j} & =\\left[\\eta_{i j}^{T}, \\gamma_{i j}^{T}\\right]^{T} \\sim N\\left(0_{15 \\times 1}, C_{i j}\\right) \\\\ \\eta_{i j} & =\\left[\\delta \\phi_{i j}^{T}, \\delta v_{i j}^{T}, \\delta p_{i j}^{T}\\right]^{T} \\sim N\\left(0_{9 \\times 1}, \\Sigma_{i j}^{\\eta}\\right) \\\\ \\gamma_{i j} & =\\left[\\delta b_{i j}^{a T}, \\delta b_{i j}^{g T}\\right]^{T} \\sim N\\left(0_{6 \\times 1}, \\Sigma_{i j}^{\\gamma}\\right) \\end{aligned} χijηijγij=[ηijT,γijT]TN(015×1,Cij)=[δϕijT,δvijT,δpijT]TN(09×1,Σijη)=[δbijaT,δbijgT]TN(06×1,Σijγ)
    根据δϕ\\delta \\phiδϕδv\\delta vδvδp\\delta pδpδba\\delta b_{a}δbaδbg\\delta b_{g}δbg,可以得到:
    Cij=(Σijη09×606×9Σijγ)15×15C_{i j}=\\left(\\begin{array}{cc} \\Sigma_{i j}^{\\eta} & 0_{9 \\times 6} \\\\ 0_{6 \\times 9} & \\Sigma_{i j}^{\\gamma} \\end{array}\\right)_{15 \\times 15} Cij=(Σijη06×909×6Σijγ)15×15

ηijΔ≜[δϕ⃗ijTδvijTδpijT]T\\boldsymbol{\\eta}_{i j}^{\\Delta} \\triangleq\\left[\\begin{array}{lll}\\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Δ=[ΔR~jj−100−ΔR~ij−1⋅(f~j−1−bia)∧ΔtI0−12ΔR~ij−1⋅(f~j−1−bia)∧Δt2ΔtII]⏟Aj−1ηij−1Δ⋯+[Jrj−1Δt00ΔR~ij−1Δt012ΔR~ij−1Δt2]⏟Bj−1[(ηj−1gd)T(ηj−1ad)T]⏟ηj−1d\\begin{aligned} \\boldsymbol{\\eta}_{i j}^{\\Delta}= &{\\underbrace{ {\\left[\\begin{array}{ccc} \\Delta \\tilde{\\mathbf{R}}_{j j-1} & \\mathbf{0} & \\mathbf{0} \\\\ -\\Delta \\tilde{\\mathbf{R}}_{i j-1} \\cdot\\left(\\tilde{\\mathbf{f}}_{j-1}-\\mathbf{b}_{i}^{a}\\right)^{\\wedge} \\Delta t & \\mathbf{I} & \\mathbf{0} \\\\ -\\frac{1}{2} \\Delta \\tilde{\\mathbf{R}}_{i j-1} \\cdot\\left(\\tilde{\\mathbf{f}}_{j-1}-\\mathbf{b}_{i}^{a}\\right)^{\\wedge} \\Delta t^{2} & \\Delta t \\mathbf{I} & \\mathbf{I} \\end{array}\\right]}} _{A_{j-1} }\\boldsymbol{\\eta}_{i j-1}^{\\Delta} \\cdots } \\\\ & +\\underbrace{\\left[\\begin{array}{cc} \\mathbf{J}_{r}^{j-1} \\Delta t & \\mathbf{0} \\\\ \\mathbf{0} & \\Delta \\tilde{\\mathbf{R}}_{i j-1} \\Delta t \\\\ \\mathbf{0} & \\frac{1}{2} \\Delta \\tilde{\\mathbf{R}}_{i j-1} \\Delta t^{2} \\end{array}\\right]}_{B_{j-1}}\\underbrace{ \\begin{bmatrix} \\left(\\eta_{j-1}^{gd}\\right)^T\\\\ \\left(\\eta_{j-1}^{ad}\\right)^T \\end{bmatrix} } _{\\boldsymbol{\\eta}_{j-1}^{d}} \\end{aligned} ηijΔ=Aj1ΔR~jj1ΔR~ij1(f~j1bia)Δt21ΔR~ij1(f~j1bia)Δt20IΔtI00Iηij1Δ+Bj1Jrj1Δt000ΔR~ij1Δt21ΔR~ij1Δt2ηj1d(ηj1gd)T(ηj1ad)T
Σijη\\Sigma_{ij}^{\\eta}Σijη递推形式如下:
Σijη=Aj−1Σij−1ηAj−1T+Bj−1ΣηBj−1T\\boldsymbol{\\Sigma}_{i j}^{\\eta}=\\mathbf{A}_{j-1} \\boldsymbol{\\Sigma}_{i j-1}^{\\eta} \\mathbf{A}_{j-1}^{T}+\\mathbf{B}_{j-1} \\boldsymbol{\\Sigma}_{\\boldsymbol{\\eta}} \\mathbf{B}_{j-1}^{T} Σijη=Aj1Σij1ηAj1T+Bj1ΣηBj1T
其中:Ση\\Sigma_{\\eta}Ση为IMU噪声的协方差矩阵

γij=[δbijaT,δbijgT]T\\gamma_{i j} =\\left[\\delta b_{i j}^{a T}, \\delta b_{i j}^{g T}\\right]^{T}γij=[δbijaT,δbijgT]T递推形式如下
γij=γij−1+σj−1d\\gamma_{i j}=\\gamma_{i j-1}+\\sigma_{j-1}^{d} γij=γij1+σj1d
其中:
σkd=[(σkbgd)T(σkbad)T]T\\sigma_{k}^{d}=\\left[\\left(\\sigma_{k}^{b g d}\\right)^{T} \\left(\\sigma_{k}^{b a d}\\right)^{T}\\right]^{T} σkd=[(σkbgd)T(σkbad)T]T
Σijγ\\Sigma_{i j}^{\\gamma}Σijγ递推形式如下:
Σijγ=Σij−1γ+Σγ\\begin{array}{l} \\Sigma_{i j}^{\\gamma}=\\Sigma_{i j-1}^{\\gamma}+\\Sigma_{\\gamma} \\end{array} Σijγ=Σij1γ+Σγ