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

已于 2023-06-11 22:54:57 修改
阅读量596 收藏 1
点赞数
分类专栏： ORB_SLAM3_源码解析 文章标签： 算法 SLAM 自动驾驶 计算机视觉 图像处理
于 2023-04-09 20:41:40 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/He3he3he/article/details/130047460
版权

在这里插入图片描述

噪声分析

  1. δ ϕ ⃗ i j \delta \vec{\phi}_{i j} δϕ ​ij​：
    Exp ⁡ ( − δ ϕ ⃗ i j ) = ∏ k = i j − 1 Exp ⁡ ( − Δ R ~ k + 1 j T ⋅ J r k ⋅ η k g d Δ 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−1​Exp(−ΔR~k+1jT​⋅Jrk​⋅ηkgd​Δt)
    其中， J r k = J r ( ( ω ~ k − b i g ) Δ 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)

对上式两边取对数：
δ ϕ ⃗ i j = − log ⁡ ( ∏ k = i j − 1 Exp ⁡ ( − Δ R ~ k + 1 j T ⋅ J r k ⋅ η k g d Δ 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−1​Exp(−ΔR~k+1jT​⋅Jrk​⋅η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(δϕ ​))=ϕ ​+Jr−1​(ϕ ​)⋅δϕ ​, 其中 η k g d \boldsymbol{\eta}_{k}^{g d} ηkgd​是小量， ξ = Δ R ~ k + 1 j T ⋅ J r k ⋅ η k g d Δ 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是小量，于是 J ⁡ r − 1 ( ξ k ) ≈ I \operatorname{J}^{-1}_{r}\left (\xi_{k} \right )\approx I Jr−1​(ξk​)≈I

δ ϕ ⃗ i j = − log ⁡ ( ∏ k = i j − 1 Exp ⁡ ( − ξ k ) ) = − log ⁡ ( Exp ⁡ ( − ξ i ) ∏ k = i + 1 j − 1 Exp ⁡ ( − ξ k ) ) ≈ − ( − ξ i + I ⋅ log ⁡ ( ∏ k = i + 1 j − 1 Exp ⁡ ( − ξ k ) ) ) = ξ i − log ⁡ ( ∏ k = i + 1 j − 1 Exp ⁡ ( − ξ k ) ) = ξ i − log ⁡ ( Exp ⁡ ( − ξ i + 1 ) ∏ k = i + 2 j − 1 Exp ⁡ ( − ξ k ) ) ≈ ξ i + ξ i + 1 − log ⁡ ( ∏ k = i + 2 j − 1 Exp ⁡ ( − ξ k ) ) ≈ ⋯ ≈ ∑ k = i j − 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−1​Exp(−ξk​))=−log(Exp(−ξi​)k=i+1∏j−1​Exp(−ξk​))≈−(−ξi​+I⋅log(k=i+1∏j−1​Exp(−ξk​)))=ξi​−log(k=i+1∏j−1​Exp(−ξk​))=ξi​−log(Exp(−ξi+1​)k=i+2∏j−1​Exp(−ξk​))≈ξi​+ξi+1​−log(k=i+2∏j−1​Exp(−ξk​))≈⋯≈k=i∑j−1​ξk​​
即：
δ ϕ ⃗ i j ≈ ∑ k = i j − 1 Δ R ~ k + 1 j T J ⁡ r k η k g d Δ 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+1jT​Jrk​ηkgd​Δt
由于 Δ R ~ k + 1 j T \Delta\tilde{R}_{k+1 j}^{T} ΔR~k+1jT​、 J ⁡ r k \operatorname{J}_{r}^{k} Jrk​、 Δ t \Delta t Δt都是已知量，而 η k g d \eta_{k}^{gd} ηkgd​为零均值高斯噪声，因此 δ ϕ ⃗ i j \delta \vec{\phi}_{i j} δϕ ​ij​也为零均值高斯噪声
2. δ v i j \delta v_{ij} δvij​
δ v i j = ∑ k = i j − 1 [ Δ R ~ i k η k a d Δ t − Δ R ~ i k ⋅ ( f ~ k − b i a ) ∧ ⋅ δ ϕ ⃗ i k ⋅ Δ 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. δ p i j \delta p_{ij} δpij​
δ p i j = ∑ k = i j − 1 [ δ v i k Δ t − 1 2 Δ R ~ i k ⋅ ( f ~ k − b i a ) ∧ δ ϕ ⃗ i k Δ t 2 + 1 2 Δ R ~ i k η k a d Δ t 2 ] \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]

噪声更新

  1. δ ϕ → i j − 1 → δ ϕ → i j \delta\overrightarrow{\phi}_{ij-1}\to \delta\overrightarrow{\phi}_{ij} δϕ ​ij−1​→δϕ ​ij​
    δ ϕ ⃗ i j = ∑ k = i j − 1 Δ R ~ k + 1 j T J r k η k g d Δ t = ∑ k = i j − 2 Δ R ~ k + 1 j T J r k η k g d Δ t + Δ R ~ j j T ⏟ I J r j − 1 η j − 1 g d Δ t = ∑ j − 2 k = i ( Δ R ~ k + 1 j − 1 Δ R ~ j − 1 j ) T J r k η k g d Δ t + J r j − 1 η j − 1 g d Δ t = Δ R ~ j j − 1 ∑ k = i j − 2 Δ R ~ k + 1 j − 1 T J r k η k g d Δ t + J r j − 1 η j − 1 g d Δ t = Δ R ~ j j − 1 δ ϕ ⃗ i j − 1 + J r j − 1 η j − 1 g d Δ 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+1jT​Jrk​ηkgd​Δt=k=i∑j−2​ΔR~k+1jT​Jrk​ηkgd​Δt+I ΔR~jjT​​​Jrj−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−1​k=i∑j−2​ΔR~k+1j−1T​Jrk​ηkgd​Δt+Jrj−1​ηj−1gd​Δt=ΔR~jj−1​δϕ ​ij−1​+Jrj−1​ηj−1gd​Δt​
  2. δ v i j − 1 → δ v i j \delta v_{ij-1} \to \delta v_{ij} δvij−1​→δvij​
    δ v i j = ∑ k = i j − 1 [ Δ R ~ i k η k a d Δ t − Δ R ~ i k ⋅ ( f ~ k − b i a ) ∧ ⋅ δ ϕ ⃗ i k ⋅ Δ t ] = ∑ k = i j − 2 [ Δ R ~ i k η k a d Δ t − Δ R ~ i k ⋅ ( f ~ k − b i a ) ∧ ⋅ δ ϕ ⃗ i k ⋅ Δ t ] … + Δ R ~ i j − 1 η j − 1 a d Δ t − Δ R ~ i j − 1 ⋅ ( f ~ j − 1 − b i a ) ∧ ⋅ δ ϕ ⃗ i j − 1 ⋅ Δ t = δ 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 \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​
  3. δ p i j − 1 → δ p i j \delta p_{ij-1} \to \delta p_{ij} δpij−1​→δpij​
    δ p i j = ∑ k = i j − 1 [ δ v i k Δ t − 1 2 Δ R ~ i k ⋅ ( f ~ k − b i a ) ∧ δ ϕ ⃗ i k Δ t 2 + 1 2 Δ R ~ i k η k a d Δ t 2 ] = δ p i j − 1 + δ v i j − 1 Δ t − 1 2 Δ R ~ i j − 1 ⋅ ( f ~ j − 1 − b i a ) ∧ δ ϕ ⃗ i j − 1 Δ t 2 + 1 2 Δ R ~ i j − 1 η j − 1 a d Δ t 2 \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:
    χ i j = [ η i j T , γ i j T ] T ∼ N ( 0 15 × 1 , C i j ) η i j = [ δ ϕ i j T , δ v i j T , δ p i j T ] T ∼ N ( 0 9 × 1 , Σ i j η ) γ i j = [ δ b i j a T , δ b i j g T ] T ∼ N ( 0 6 × 1 , Σ i j γ ) \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、 δ b a \delta b_{a} δba​、 δ b g \delta b_{g} δbg​，可以得到：
    C i j = ( Σ i j η 0 9 × 6 0 6 × 9 Σ i j γ ) 15 × 15 C_{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×9​​09×6​Σijγ​​)15×15​

η i j Δ ≜ [ δ ϕ ⃗ i j T δ v i j T δ p i j T ] 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的递推形式如下
η i j Δ = [ Δ R ~ j j − 1 0 0 − Δ R ~ i j − 1 ⋅ ( f ~ j − 1 − b i a ) ∧ Δ t I 0 − 1 2 Δ R ~ i j − 1 ⋅ ( f ~ j − 1 − b i a ) ∧ Δ t 2 Δ t I I ] ⏟ A j − 1 η i j − 1 Δ ⋯ + [ J r j − 1 Δ t 0 0 Δ R ~ i j − 1 Δ t 0 1 2 Δ R ~ i j − 1 Δ t 2 ] ⏟ B j − 1 [ ( η j − 1 g d ) T ( η j − 1 a d ) T ] ⏟ η j − 1 d \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​)∧Δt2​0IΔtI​00I​ ​​​ηij−1Δ​⋯+Bj−1​ ​Jrj−1​Δt00​0ΔR~ij−1​Δt21​ΔR~ij−1​Δt2​ ​​​ηj−1d​ ​(ηj−1gd​)T(ηj−1ad​)T​ ​​​​
Σ i j η \Sigma_{ij}^{\eta} Σijη​的递推形式如下：
Σ i j η = A j − 1 Σ i j − 1 η A j − 1 T + B j − 1 Σ η B j − 1 T \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噪声的协方差矩阵

γ i j = [ δ b i j a T , δ b i j g T ] T \gamma_{i j} =\left[\delta b_{i j}^{a T}, \delta b_{i j}^{g T}\right]^{T} γij​=[δbijaT​,δbijgT​]T的递推形式如下
γ i j = γ i j − 1 + σ j − 1 d \gamma_{i j}=\gamma_{i j-1}+\sigma_{j-1}^{d} γij​=γij−1​+σj−1d​
其中：
σ k d = [ ( σ k b g d ) T ( σ k b a d ) 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
Σ i j γ \Sigma_{i j}^{\gamma} Σijγ​的递推形式如下：
Σ i j γ = Σ i j − 1 γ + Σ γ \begin{array}{l} \Sigma_{i j}^{\gamma}=\Sigma_{i j-1}^{\gamma}+\Sigma_{\gamma} \end{array} Σijγ​=Σij−1γ​+Σγ​​

火柴的初心
关注
专栏目录
【逐函数讲解ORB_SLAM3源码】获取普通帧率、关键帧、IMU数据
小秋 AI SLAM 入门实战教程
05-12 831
获得imu的旋转 mRwc * mImuCalib.mTcb.rotationMatrix(); Sophus::SE3 mTwc 创建ORB_SLAM3系统 Create SLAM system. It initializes all system threads and gets ready to process frames. ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_RGBD, true, 0, file_nam
【ORB_SLAM3源码解读】IMU基础介绍、IMU姿态、速度、位置解算以及误差方程、坐标系
小秋 AI SLAM 入门实战教程
04-29 9042
ORB_SLAM3优势 IMU(Inertial Measurement Unit),惯性测量单元 • 典型 6 轴 IMU 以较高频率(≥ 100Hz)返回被测量物体的角速度与加速度 • 受自身温度、零偏、振动等因素干扰,积分得到的平移和旋转容易漂移 六自由度 IMU 本身由一个陀螺仪和一个加速度计组成,分别测量自身的角速度和加速度。 IMU 适合计算短时间、快速的运动; 快速响应,不受成像质量影响,角速度普遍比较准确,可估计绝对尺度 存在零偏,低精度 IMU 积分位姿发散,高精度价格昂贵 可利用视觉定位
滤波中imu噪声设置注意事项
chennuo0125的博客
09-18 460
imu噪声设置注意事项
ORBSLAM3初始化对配置文件里的IMU标定参数（噪声和随机游走）这么敏感，是拿那些参数参与实际的IMU预积分运算了
最新发布
诗筱涵的博客
07-23 820
Nga 和NgaWalk 有在Preintegrated::IntegrateNewMeasurement函数里更新协方差的时候用到，协方差也就是信息矩阵，程小六的书里有讲（这感觉跟vins里面影响IMU预积分权重类似啊 ），Preintegrated::IntegrateNewMeasurement函数就是预积分计算函数（可以看这里的注释。ORBSLAM3初始化对配置文件里的IMU标定参数（噪声和随机游走）这么敏感，是拿那些参数参与实际的IMU预积分运算了，vins里面那些参数只是影响IMU预积分权重。
ORB_SLAM3_IMU预积分理论推导(预积分项)
火柴的初心的博客
04-09 1258
误差 1.确定性误差 Bias：偏置 Scale：实际数值和传感器输出值之间的比值。 Misalignment：非正交误差。 标定的方法：六面法标定加速度。 2.随机误差 Allan方差 随机游走 IMU器件测量模型 1.角标符号说明 b：body坐标系 a：加速计(acc) g：陀螺仪(gyro) w：世界坐标系 d：离散(discrete) 2.假设 假设：地球是静止的，忽略自转，运行平面为水平面，重力指向固定，模值恒定。 运行场景小； 运行时间短； 精度低(mems)； 3.测量
ORB-SLAM3中的IMU预积分
qq_49561752的博客
04-13 753
IMU预积分之前也博客也提到几次了，这次把ORB-SLAM3中相应的流程记录下，与推导公式一一对应，顺带复习。
ORB_SLAM3 IMU预积分PreintegrateIMU
火柴的初心的博客
06-22 798
这篇博文主要分享ORB_SLAM3中Tracking::PreintegrateIMU()，其主要包括几个部分： 获得两帧之间的IMU数据 中值积分 IMU状态更新 关于IMU的理论推导参考： ORB_SLAM3_IMU预积分理论推导(预积分项) ORB_SLAM3_IMU预积分理论推导(噪声分析) ORB_SLAM3_IMU预积分理论推导(更新) ORB_SLAM3_IMU预积分理论推导(残差) 1.获得两帧之间的IMU数据 主要是为了获取当前帧和前一帧之间的IMU数据，以便用于后续的IMU预积分
ORB_SLAM3_IMU预积分理论推导(残差)
火柴的初心的博客
04-10 741
残差 预积分的测量值采用一阶近似修正，免去了积分重新运算，降低计算量 状态包含：Ri,pi,vi,Rj,pj,vj,δbig,δbia\mathbf{R}_{i}, \mathbf{p}_{i}, \mathbf{v}_{i}, \mathbf{R}_{j}, \mathbf{p}_{j}, \mathbf{v}_{j}, \delta \mathbf{b}_{i}^{g}, \delta \mathbf{b}_{i}^{a}Ri​,pi​,vi​,Rj​,pj​,vj​,δbig​,δbia​，其中关于
结合orb_slam3中预积分相关代码和预积分原理推导一遍orb_slam3中的预积分过程
04-03
ORB-SLAM3中的预积分过程是基于IMU（惯性测量单元）数据的，主要用于在视觉SLAM中融合IMU数据，提高位姿估计的精度和鲁棒性。预积分过程包含三个主要步骤：预积分初始化、预积分更新和预积分优化。 1. 预积分初始化...
imu预积分总结和推导.pdf
10-27
例如，在VI-ORBSLAM、港科大VINS、百度/浙大ICE-BA等知名VIO系统中，都可见到IMU预积分技术的应用。 为了更好地理解IMU预积分理论，首先需要掌握传统捷联惯性导航的基本知识。惯性导航依赖于牛顿第二定律，其核心...
SLAM_关于VIO中IMU预积分的讲解
惊鸿一博
04-02 4723
1. 为什么要做VIO? 首先我们先简单回顾一下，以及为什么要做VIO初始化。 我们知道单目相机在做SLAM的时候 a. 是缺乏实际的尺度信息的，而且尺度还会产生漂移。如果缺乏实际的尺度信息，此时的定位信息将很难进行实际的应用。 b. 在面对一些极端环境的时候或者突然运动过快的时候，视觉还可能直接跟丢。 c. 还有一点就是，纯视觉的SLAM系统如果不做特殊处理，是无法知道惯性坐标系的，视觉SLAM出的pose大多都是基于第一帧为世界坐标系建立的，所以就有可能出现车明明是在地上开，出的轨迹看上去...
3.1ORBSLAM3之IMU预积分
超爱吃小蛋糕的66的博客
12-12 954
对ORB-SLAM3代码中的IMU预积分模块进行梳理
ORB_SLAM3中IMU预积分过程原理分析
chennuo0125的博客
01-07 2062
ORB_SLAM3中IMU预积分过程原理分析 1. 特殊正交群SO(3)的一些性质 指数映射: exp(ϕ^)=I+sin(∥ϕ∥)∥ϕ∥ϕ^+1−cos(∥ϕ∥)∥ϕ∥2(ϕ^)2(1) exp(\phi^{\hat{ }}) = \mathbf{I} + \frac{sin(\left\|\phi \right\|)}{\left\|\phi \right\|}\phi^{\hat{ }} + \frac{1-cos(\left\|\phi \right\|)}{\left\|\phi \right\|
《ORB-SLAM3》IMU初始化
jjjqqq123321的博客
01-23 1537
刚接触了IMU传感器，除了IMU预积分的相关知识外，关于IMU的另一个重要知识点就是IMU的初始化。IMU为什么需要初始化？IMU初始化是为了获得重力方向和IMU零偏的初始值。有了正确的重力方向才能消除IMU预积分中加速度计关于重力的影响，得到的IMU预积分数据才能保证准确。
重磅直播 | ORB-SLAM3系列代码讲解：IMU 专题三
3D视觉工坊
11-08 419
ORB-SLAM笔记——ORB-SLAM3的IMU初始化（1）理论部分
RoBOt__Dreamer
07-28 7982
上一篇blog立的flag，哎最近工作虽然也是快忙炸了但面对这么好的一个作品，你就忍不住挤时间也要学他啊哈哈。那么我们开始！ 在正式说它的IMU初始化部分之前，还是跟随作者论文的思路，咱们再把Visual-Inertial的架构梳理一下。 首先对于一个纯视觉的系统来说，单目也好，双目也罢，我们主要关心的还是每一个pose的6Dof，对应的状态空间呢也就是六维。考虑到单目的尺度问题，状态空间也就是多了一个scale。但在一个visual-inertial SLAM系统中，我们在对当前pose进行估计的同时
ORBSLAM3的IMU积分求解过程
10-20 3995
目前是自己的手稿版的ORBSLAM3的最大后验概率推导, 主要是参考论文: <Inertial-Only Optimization for Visual-Inertial Initialization> 后面找到大块的时间编辑一下, 大家可以先收藏,然后互相交流~~
重磅直播 | ORB-SLAM3系列代码讲解：IMU 专题二
3D视觉工坊
09-17 358
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

火柴的初心

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

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

余额充值