ORB_SLAM3_IMU预积分理论推导(噪声分析)
噪声分析
- δϕ⃗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=i∏j−1Exp(−ΔR~k+1jT⋅Jrk⋅η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((ω~k−big)Δ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=i∏j−1Exp(−ΔR~k+1jT⋅Jrk⋅ηkgdΔt))
利用性质:当δϕ⃗\\delta \\vec{\\phi}δϕ是小量时,log(Exp(ϕ⃗)⋅Exp(δϕ⃗))=ϕ⃗+Jr−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(δϕ))=ϕ+Jr−1(ϕ)⋅δϕ, 其中η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+1jT⋅Jrk⋅ηkgdΔt是小量,于是Jr−1(ξk)≈I\\operatorname{J}^{-1}_{r}\\left (\\xi_{k} \\right )\\approx IJr−1(ξ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=i∏j−1Exp(−ξk))=−log(Exp(−ξi)k=i+1∏j−1Exp(−ξk))≈−(−ξi+I⋅log(k=i+1∏j−1Exp(−ξk)))=ξi−log(k=i+1∏j−1Exp(−ξk))=ξi−log(Exp(−ξi+1)k=i+2∏j−1Exp(−ξk))≈ξi+ξi+1−log(k=i+2∏j−1Exp(−ξk))≈⋯≈k=i∑j−1ξk
即:
δϕ⃗ij≈∑k=ij−1ΔR~k+1jTJrkη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 } δϕij≈k=i∑j−1ΔR~k+1jTJrkηkgdΔt
由于ΔR~k+1jT\\Delta\\tilde{R}_{k+1 j}^{T}ΔR~k+1jT、Jrk\\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=i∑j−1[ΔR~ikηkadΔt−ΔR~ik⋅(f~k−bia)∧⋅δϕ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=i∑j−1[δvikΔt−21ΔR~ik⋅(f~k−bia)∧δϕikΔt2+21ΔR~ikηkadΔt2]
噪声更新
- δϕ→ij−1→δϕ→ij\\delta\\overrightarrow{\\phi}_{ij-1}\\to \\delta\\overrightarrow{\\phi}_{ij}δϕij−1→δϕ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=i∑j−1ΔR~k+1jTJrkηkgdΔt=k=i∑j−2ΔR~k+1jTJrkηkgdΔt+IΔR~jjTJrj−1ηj−1gdΔt=j−2∑k=i(ΔR~k+1j−1ΔR~j−1j)TJrkηkgdΔt+Jrj−1ηj−1gdΔt=ΔR~jj−1k=i∑j−2ΔR~k+1j−1TJrkηkgdΔt+Jrj−1ηj−1gdΔt=ΔR~jj−1δϕij−1+Jrj−1ηj−1gdΔt - δvij−1→δvij\\delta v_{ij-1} \\to \\delta v_{ij}δvij−1→δ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=i∑j−1[ΔR~ikηkadΔt−ΔR~ik⋅(f~k−bia)∧⋅δϕik⋅Δt]k=i∑j−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 - δpij−1→δpij\\delta p_{ij-1} \\to \\delta p_{ij}δpij−1→δ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=i∑j−1[δvikΔt−21ΔR~ik⋅(f~k−bia)∧δϕikΔt2+21ΔR~ikηkadΔt2]=δpij−1+δvij−1Δt−21ΔR~ij−1⋅(f~j−1−bia)∧δϕij−1Δt2+21ΔR~ij−1ηj−1adΔ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]T∼N(015×1,Cij)=[δϕijT,δvijT,δpijT]T∼N(09×1,Σijη)=[δbijaT,δbijgT]T∼N(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Δ=Aj−1ΔR~jj−1−ΔR~ij−1⋅(f~j−1−bia)∧Δt−21ΔR~ij−1⋅(f~j−1−bia)∧Δt20IΔtI00Iηij−1Δ⋯+Bj−1Jrj−1Δt000ΔR~ij−1Δt21ΔR~ij−1Δt2ηj−1d(ηj−1gd)T(ηj−1ad)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η=Aj−1Σij−1ηAj−1T+Bj−1ΣηBj−1T
其中:Ση\\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=γij−1+σj−1d
其中:
σ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γ=Σij−1γ+Σγ