厦门企业网站开发公司,大连网络工程,二级学院网站建设报告,营销推广企业本章开始将会开始对IMU预积分进行详细的推导。本次的推导采用李群李代数的形式来表示旋转#xff0c;对应的是ORB-SLAM3的方案。比如VINS#xff0c;采用的四元数方案#xff0c;可能从推导过程上看有一些不同。 IMU预积分推导
先说一下李群李代数的定义吧#xff1a; R ˙…本章开始将会开始对IMU预积分进行详细的推导。本次的推导采用李群李代数的形式来表示旋转对应的是ORB-SLAM3的方案。比如VINS采用的四元数方案可能从推导过程上看有一些不同。 IMU预积分推导
先说一下李群李代数的定义吧 R ˙ ϕ ∧ R \dot{\mathbf{R}}\phi^{\wedge}\mathbf{R} R˙ϕ∧R R Exp ( ϕ ) exp ( ϕ ∧ ) \mathbf{R}\operatorname{Exp}(\phi)\operatorname{exp}(\phi^{\wedge }) RExp(ϕ)exp(ϕ∧)
其中 R \mathbf{R} R为李群SO(3) ϕ \phi ϕ为对应的李代数so(3)两者之间相差一个指数对数的关系。如果是 Exp \operatorname{Exp} Exp括号里面是李代数向量如果是 exp \operatorname{exp} exp括号里面是李代数向量对应的反对称矩阵。
有时候会有用 ϕ ⃗ \vec{\phi} ϕ 的表示其实与 ϕ \phi ϕ的含义是一致的。
PVQ增量真值
假设 i i i帧的Q、V、P分别是 R i R_i Ri、 v i v_i vi、 p i p_i pi则可以利用从 k i ki ki到 k j − 1 kj-1 kj−1帧的所有IMU测量值直接更新得到 j j j帧的 R j R_j Rj、 v j v_j vj、 p j p_j pj详细如下 R j R i ⋅ ∏ k i j − 1 Exp ( ( ω ~ k − b k g − η k g ) ⋅ Δ t ) \mathbf{R}_{j}\mathbf{R}_{i} \cdot \prod_{ki}^{j-1} \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{k}^{g}-\boldsymbol{\eta}_{k}^{g}\right) \cdot \Delta t\right) RjRi⋅ki∏j−1Exp((ω~k−bkg−ηkg)⋅Δt)
其中 ω ~ k \tilde{\boldsymbol{\omega}}_{k} ω~k是第 k k k帧的角速度的测量值 b k g \mathbf{b}_{k}^{g} bkg是第 k k k帧的角速度零偏 η k g \boldsymbol{\eta}_{k}^{g} ηkg是第 k k k帧的角速度测量噪声。 v j v i g ⋅ Δ t i j ∑ k i j − 1 R k ⋅ ( a ~ k − b k a − η k a ) ⋅ Δ t \mathbf{v}_{j}\mathbf{v}_{i}\mathbf{g} \cdot \Delta t_{i j}\sum_{ki}^{j-1} \mathbf{R}_{k} \cdot\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{k}^{a}-\mathbf{\eta}_{k}^{a}\right) \cdot \Delta t vjvig⋅Δtijki∑j−1Rk⋅(a~k−bka−ηka)⋅Δt
其中 g g g是重力加速度 a ~ k \tilde{\mathbf{a}}_{k} a~k是第 k k k帧的加速度的测量值 b k a \mathbf{b}_{k}^{a} bka是第 k k k帧的加速度零偏 η k a \boldsymbol{\eta}_{k}^{a} ηka是第 k k k帧的加速度测量噪声。加速度的测量值耦合了重力加速度因此需要加上一个 g ⋅ Δ t i j \mathbf{g} \cdot \Delta t_{i j} g⋅Δtij进行抵消。 p j p i ∑ k i j − 1 [ v k ⋅ Δ t 1 2 g ⋅ Δ t 2 1 2 R k ⋅ ( a ~ k − b k a − η k a ) ⋅ Δ t 2 ] \begin{aligned} \mathbf{p}_{j} \mathbf{p}_{i}\sum_{ki}^{j-1}\left[\mathbf{v}_{k} \cdot \Delta t\frac{1}{2} \mathbf{g} \cdot \Delta t^{2}\frac{1}{2} \mathbf{R}_{k} \cdot\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{k}^{a}-\mathbf{\eta}_{k}^{a}\right) \cdot \Delta t^{2}\right] \end{aligned} pjpiki∑j−1[vk⋅Δt21g⋅Δt221Rk⋅(a~k−bka−ηka)⋅Δt2]
加速度的测量值耦合了重力加速度因此需要加上一个 1 2 g ⋅ Δ t 2 \frac{1}{2} \mathbf{g} \cdot \Delta t^{2} 21g⋅Δt2进行抵消。
同时 Δ t i j ∑ k i j − 1 Δ t ( j − i ) Δ t \Delta t_{ij}\sum_{ki}^{j-1}\Delta t(j-i)\Delta t Δtijki∑j−1Δt(j−i)Δt
为了避免每次更新初始的 R i R_i Ri、 v i v_i vi、 p i p_i pi都要重新积分求解 R j R_j Rj、 v j v_j vj、 p j p_j pj引出PVQ增量真值即预积分的值。详细如下这里应用了正交矩阵旋转矩阵的转置等于正交矩阵的逆的性质 Δ R i j ≜ R i T R j ∏ k i j − 1 Exp ( ( ω ~ k − b k g − η k g ) ⋅ Δ t ) \begin{aligned} \Delta \mathbf{R}_{i j} \triangleq \mathbf{R}_{i}^{T} \mathbf{R}_{j} \\ \prod_{ki}^{j-1} \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{k}^{g}-\boldsymbol{\eta}_{k}^{g}\right) \cdot \Delta t\right) \end{aligned} ΔRij≜RiTRjki∏j−1Exp((ω~k−bkg−ηkg)⋅Δt) Δ v i j ≜ R i T ( v j − v i − g ⋅ Δ t i j ) ∑ k i j − 1 Δ R i k ⋅ ( a ~ k − b k a − η k a ) ⋅ Δ t \begin{aligned} \Delta \mathbf{v}_{i j} \triangleq \mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right) \\ \sum_{ki}^{j-1} \Delta \mathbf{R}_{i k} \cdot\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{k}^{a}-\mathbf{\eta}_{k}^{a}\right) \cdot \Delta t \end{aligned} Δvij≜RiT(vj−vi−g⋅Δtij)ki∑j−1ΔRik⋅(a~k−bka−ηka)⋅Δt Δ p i j ≜ R i T ( p j − p i − v i ⋅ Δ t i j − 1 2 g ⋅ Δ t i j 2 ) ∑ k i j − 1 [ Δ v i k ⋅ Δ t 1 2 Δ R i k ⋅ ( a ~ k − b k a − η k a ) ⋅ Δ t 2 ] \begin{aligned} \Delta \mathbf{p}_{i j} \triangleq \mathbf{R}_{i}^{T}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \cdot \Delta t_{i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right) \\ \sum_{ki}^{j-1}\left[\Delta \mathbf{v}_{i k} \cdot \Delta t\frac{1}{2} \Delta \mathbf{R}_{i k} \cdot\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{k}^{a}-\boldsymbol{\eta}_{k}^{a}\right) \cdot \Delta t^{2}\right] \end{aligned} Δpij≜RiT(pj−pi−vi⋅Δtij−21g⋅Δtij2)ki∑j−1[Δvik⋅Δt21ΔRik⋅(a~k−bka−ηka)⋅Δt2]
注意上面公式中的 Δ v i j \Delta \mathbf{v}_{i j} Δvij、 Δ p i j \Delta \mathbf{p}_{i j} Δpij并不是通常意义上的速度和位置变化量而是根据IMU加速度计的测量值计算出来的所谓的位移和速度增量由于IMU加速度测量值耦合了重力加速度因此对应的IMU预积分真值也必须含有一个重力加速度的分量否则无法解释速度的变化量为什么还要减去 g ⋅ Δ t i j \mathbf{g} \cdot \Delta t_{i j} g⋅Δtij位置的变化量为什么还要减去 1 2 g ⋅ Δ t i j 2 \frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2} 21g⋅Δtij2。
由积分引出预积分预积分里面的每一项与起始状态无关可以认为都是相对量这个好处在于计算预积分时不需要考虑起始状态值得注意的是关于速度与位置的预积分里面都包含了重力。这部分相对量就是PVQ增量真值。
即PVQ增量真值的核心就是消除起始状态对积分的影响同时保留重力的影响。
上面将PVQ增量真值表达成传感器增量测量值减去它的零偏与噪声其中零偏可以作为状态量去得出但是噪声是没有办法得出的。通常的办法就是通过计算噪声的方式来将其过滤掉当然无论是优化还是滤波都逃不过一个重要的矩阵——信息矩阵协方差矩阵的逆。由于假设了传感器噪声 η k g \mathbf{\eta}_{k}^{g} ηkg、 η k a \mathbf{\eta}_{k}^{a} ηka是高斯白噪声所以传感器噪声的方差对PVQ增量噪声的方差 δ ϕ ⃗ i j \delta\vec{\phi}_{i j} δϕ ij、 δ v i j \delta\mathbf{v}_{i j} δvij、 δ p i j \delta\mathbf{p}_{i j} δpij的影响可以通过高斯分布推理过来的。即推导出PVQ增量噪声关于传感器噪声的式子还要推出其协方差之间的关系。
PVQ增量噪声分离
由于 P V Q 增量真值 P V Q 增量测量值 − P V Q 增量噪声 PVQ增量真值PVQ增量测量值-PVQ增量噪声 PVQ增量真值PVQ增量测量值−PVQ增量噪声
下面分别对 Δ R i j \Delta \mathbf{R}_{i j} ΔRij、 Δ v i j \Delta \mathbf{v}_{i j} Δvij、 Δ p i j \Delta \mathbf{p}_{i j} Δpij进行整理尝试将噪声项 η k g \boldsymbol{\eta}_{k}^{g} ηkg、 η k a \boldsymbol{\eta}_{k}^{a} ηka从PVQ增量测量值中分离出来使PVQ增量真值具有上述的形式以便于后续推导出噪声的递推公式。
假设在预积分的区间内两帧间的零偏是相等的即 b i g b i 1 g ⋯ b j g \mathbf{b}_{i}^{g}\mathbf{b}_{i1}^{g}\cdots\mathbf{b}_{j}^{g} bigbi1g⋯bjg以及 b i a b i 1 a ⋯ b j a \mathbf{b}_{i}^{a}\mathbf{b}_{i1}^{a}\cdots\mathbf{b}_{j}^{a} biabi1a⋯bja。 Δ R i j \Delta \mathbf{R}_{i j} ΔRij真值分离
对于 Δ R i j \Delta \mathbf{R}_{i j} ΔRij则有 Δ R i j ∏ k i j − 1 Exp ( ( ω ~ k − b i g ) Δ t − η k g Δ t ) ≈ 1 ∏ k i j − 1 { Exp ( ( ω ~ k − b i g ) Δ t ) ⋅ Exp ( − J r k ⋅ η k g Δ t ) } 2 Exp ( ( ω ~ i − b i g ) Δ t ) ⋅ Exp ( − J r i ⋅ η i g Δ t ) ⋅ Exp ( ( ω ~ i 1 − b i g ) Δ t ) ⋅ Exp ( − J r i 1 ⋅ η i 1 g Δ t ) . . . Δ R ~ i , i 1 ⋅ Exp ( − J r i ⋅ η i g Δ t ) ⋅ Δ R ~ i 1 , i 2 ⋅ Exp ( − J r i 1 ⋅ η i 1 g Δ t ) ⋅ Δ R ~ i 2 , i 3 . . . 3 Δ R ~ i , i 1 ⋅ Δ R ~ i 1 , i 2 ⋅ Exp ( − Δ R ~ i 1 , i 2 T ⋅ J r i ⋅ η i g Δ t ) ⋅ Exp ( − J r i 1 ⋅ η i 1 g Δ t ) ⋅ Δ R ~ i 2 , i 3 . . . 4 Δ R ~ i , j ⋅ Exp ( − Δ R ~ i 1 , j T ⋅ J r i ⋅ η i g Δ t ) ⋅ Exp ( − Δ R ~ i 2 , j T ⋅ J r i 1 ⋅ η i 1 g Δ t ) . . . Δ R ~ i j ⋅ ∏ k i j − 1 Exp ( − Δ R ~ k 1 j T ⋅ J r k ⋅ η k g Δ t ) \begin{aligned} \Delta \mathbf{R}_{i j} \prod_{ki}^{j-1} \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{i}^{g}\right) \Delta t-\mathbf{\eta}_{k}^{g} \Delta t\right) \\ \stackrel{1}\approx \prod_{ki}^{j-1}\left\{\operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{i}^{g}\right) \Delta t\right) \cdot \operatorname{Exp}\left(-\mathbf{J}_{r}^{k} \cdot \mathbf{\eta}_{k}^{g} \Delta t\right)\right\} \\ \stackrel{2} \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{i}-\mathbf{b}_{i}^{g}\right) \Delta t\right) \cdot \operatorname{Exp}\left(-\mathbf{J}_{r}^i \cdot \mathbf{\eta}_{i}^{g} \Delta t\right) \cdot \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{i1}-\mathbf{b}_{i}^{g}\right) \Delta t\right) \cdot \operatorname{Exp}\left(-\mathbf{J}_{r}^{i1} \cdot \mathbf{\eta}_{i1}^{g} \Delta t\right) ...\\ \Delta \tilde{\mathbf{R}}_{i,i1} \cdot \operatorname{Exp}\left(-\mathbf{J}_{r}^i \cdot \mathbf{\eta}_{i}^{g} \Delta t\right) \cdot \Delta \tilde{\mathbf{R}}_{i1,i2} \cdot \operatorname{Exp}\left(-\mathbf{J}_{r}^{i1} \cdot \mathbf{\eta}_{i1}^{g} \Delta t\right) \cdot \Delta \tilde{\mathbf{R}}_{i2,i3} ...\\ \stackrel{3} \Delta \tilde{\mathbf{R}}_{i,i1} \cdot \Delta \tilde{\mathbf{R}}_{i1,i2} \cdot \operatorname{Exp}\left(-\Delta \tilde{\mathbf{R}}_{i1,i2}^T \cdot \mathbf{J}_{r}^i \cdot \mathbf{\eta}_{i}^{g} \Delta t\right) \cdot \operatorname{Exp}\left(-\mathbf{J}_{r}^{i1} \cdot \mathbf{\eta}_{i1}^{g} \Delta t\right) \cdot \Delta \tilde{\mathbf{R}}_{i2,i3}...\\ \stackrel{4} \Delta \tilde{\mathbf{R}}_{i,j} \cdot \operatorname{Exp}\left(-\Delta \tilde{\mathbf{R}}_{i1,j}^T \cdot \mathbf{J}_{r}^i \cdot \mathbf{\eta}_{i}^{g} \Delta t\right) \cdot \operatorname{Exp}\left(-\Delta \tilde{\mathbf{R}}_{i2,j}^T \cdot \mathbf{J}_{r}^{i1} \cdot \mathbf{\eta}_{i1}^{g} \Delta t\right)...\\ \Delta \tilde{\mathbf{R}}_{i j} \cdot \prod_{ki}^{j-1} \operatorname{Exp}\left(-\Delta \tilde{\mathbf{R}}_{k1 j}^{T} \cdot \mathbf{J}_{r}^{k} \cdot \boldsymbol{\eta}_{k}^{g} \Delta t\right) \end{aligned} ΔRijki∏j−1Exp((ω~k−big)Δt−ηkgΔt)≈1ki∏j−1{Exp((ω~k−big)Δt)⋅Exp(−Jrk⋅ηkgΔt)}2Exp((ω~i−big)Δt)⋅Exp(−Jri⋅ηigΔt)⋅Exp((ω~i1−big)Δt)⋅Exp(−Jri1⋅ηi1gΔt)...ΔR~i,i1⋅Exp(−Jri⋅ηigΔt)⋅ΔR~i1,i2⋅Exp(−Jri1⋅ηi1gΔt)⋅ΔR~i2,i3...3ΔR~i,i1⋅ΔR~i1,i2⋅Exp(−ΔR~i1,i2T⋅Jri⋅ηigΔt)⋅Exp(−Jri1⋅ηi1gΔt)⋅ΔR~i2,i3...4ΔR~i,j⋅Exp(−ΔR~i1,jT⋅Jri⋅ηigΔt)⋅Exp(−ΔR~i2,jT⋅Jri1⋅ηi1gΔt)...ΔR~ij⋅ki∏j−1Exp(−ΔR~k1jT⋅Jrk⋅ηkgΔt)
其中1处使用了BCH近似性质当 δ ϕ ⃗ \delta\vec{\phi} δϕ 是小量时 Exp ( ϕ ⃗ δ ϕ ⃗ ) ≈ Exp ( ϕ ⃗ ) ⋅ Exp ( J r ( ϕ ⃗ ) ⋅ δ ϕ ⃗ ) \operatorname{Exp}(\vec{\phi}\delta \vec{\phi}) \approx \operatorname{Exp}(\vec{\phi}) \cdot \operatorname{Exp}\left(\mathbf{J}_{r}(\vec{\phi}) \cdot \delta \vec{\phi}\right) Exp(ϕ δϕ )≈Exp(ϕ )⋅Exp(Jr(ϕ )⋅δϕ ) Exp ( ϕ ⃗ J r − 1 ( ϕ ⃗ ) ⋅ δ ϕ ⃗ ) ≈ Exp ( ϕ ⃗ ) ⋅ Exp ( δ ϕ ⃗ ) \operatorname{Exp}\left(\vec{\phi} \mathbf{J}^{-1}_{r}(\vec{\phi}) \cdot \delta \vec{\phi}\right) \approx \operatorname{Exp}(\vec{\phi})\cdot \operatorname{Exp}(\delta \vec{\phi}) Exp(ϕ Jr−1(ϕ )⋅δϕ )≈Exp(ϕ )⋅Exp(δϕ )
其中2处将 ∏ \prod ∏展开3和4处利用Adjoint性质将所有的 Δ R ~ \Delta\tilde{\mathbf{R}} ΔR~ 换到最左侧这里需要注意 Exp ( ) \operatorname{Exp} \left(\right) Exp() 内部的 Δ R ~ \Delta\tilde{\mathbf{R}} ΔR~的下标和数量 Exp ( ϕ ⃗ ) ⋅ R R ⋅ Exp ( R T ϕ ⃗ ) \operatorname{Exp}(\vec{\phi}) \cdot \mathbf{R}\mathbf{R} \cdot \operatorname{Exp}\left(\mathbf{R}^{T} \vec{\phi}\right) Exp(ϕ )⋅RR⋅Exp(RTϕ )
令 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) JrkJr((ω~k−big)Δt) Δ R ~ i j ∏ k i j − 1 Exp ( ( ω ~ k − b i g ) Δ t ) \Delta \tilde{\mathbf{R}}_{i j}\prod_{ki}^{j-1} \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{i}^{g}\right) \Delta t\right) ΔR~ijki∏j−1Exp((ω~k−big)Δt) Exp ( − δ ϕ ⃗ i j ) ∏ k i j − 1 Exp ( − Δ R ~ k 1 j T ⋅ J r k ⋅ η k g Δ t ) \operatorname{Exp}\left(-\delta \vec{\phi}_{i j}\right)\prod_{ki}^{j-1} \operatorname{Exp}\left(-\Delta \tilde{\mathbf{R}}_{k1 j}^{T} \cdot \mathbf{J}_{r}^{k} \cdot \mathbf{\eta}_{k}^{g} \Delta t\right) Exp(−δϕ ij)ki∏j−1Exp(−ΔR~k1jT⋅Jrk⋅ηkgΔt)
则有 Δ R i j ≜ Δ R ~ i j ⋅ Exp ( − δ ϕ ⃗ i j ) \Delta \mathbf{R}_{i j} \triangleq \Delta \tilde{\mathbf{R}}_{i j} \cdot \operatorname{Exp}\left(-\delta \vec{\phi}_{i j}\right) ΔRij≜ΔR~ij⋅Exp(−δϕ ij) Δ R ~ i j \Delta\tilde{\mathbf{R}}_{i j} ΔR~ij即PVQ增量测量值它由陀螺仪测量值和对陀螺仪零偏的估计得到而 δ ϕ ⃗ i j \delta\vec{\phi}_{i j} δϕ ij或 Exp ( δ ϕ ⃗ i j ) \operatorname{Exp}\left(\delta\vec{\phi}_{i j}\right) Exp(δϕ ij) 即测量噪声。 Δ v i j \Delta\mathbf{v}_{i j} Δvij真值分离
将 Δ R i j \Delta \mathbf{R}_{i j} ΔRij真值整理好的式子带入 Δ v i j \Delta\mathbf{v}_{i j} Δvij真值式子中进行整理 Δ v i j ∑ k i j − 1 Δ R i k ⋅ ( a ~ k − b i a − η k a ) ⋅ Δ t ≈ ∑ k i j − 1 Δ R ~ i k ⋅ Exp ( − δ ϕ ⃗ i k ) ⋅ ( a ~ k − b i a − η k a ) ⋅ Δ t ≈ 1 ∑ k i j − 1 Δ R ~ i k ⋅ ( I − δ ϕ ⃗ i k ∧ ) ⋅ ( a ~ k − b i a − η k a ) ⋅ Δ t ≈ 2 ∑ k i j − 1 [ Δ R ~ i k ⋅ ( I − δ ϕ ⃗ i k ∧ ) ⋅ ( a ~ k − b i a ) ⋅ Δ t − Δ R ~ i k η k a Δ t ] 3 ∑ k i j − 1 [ Δ R ~ i k ⋅ ( a ~ k − b i a ) ⋅ Δ t Δ R ~ i k ⋅ ( a ~ k − b i a ) ∧ ⋅ δ ϕ ⃗ i k ⋅ Δ t − Δ R ~ i k η k a Δ t ] ∑ k i j − 1 [ Δ R ~ i k ⋅ ( a ~ k − b i a ) ⋅ Δ t ] ∑ k i j − 1 [ Δ R ~ i k ⋅ ( a ~ k − b i a ) ∧ ⋅ δ ϕ ⃗ i k ⋅ Δ t − Δ R ~ i k η k a Δ t ] \begin{aligned} \Delta \mathbf{v}_{i j} \sum_{ki}^{j-1} \Delta \mathbf{R}_{i k} \cdot\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}-\mathbf{\eta}_{k}^{a}\right) \cdot \Delta t \\ \approx \sum_{ki}^{j-1} \Delta \tilde{\mathbf{R}}_{i k} \cdot \operatorname{Exp}\left(-\delta \vec{\phi}_{i k}\right) \cdot\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}-\mathbf{\eta}_{k}^{a}\right) \cdot \Delta t \\ \stackrel{1}\approx \sum_{ki}^{j-1} \Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\mathbf{I}-\delta \vec{\phi}_{i k}^{\wedge}\right) \cdot\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}-\boldsymbol{\eta}_{k}^{a}\right) \cdot \Delta t \\ \stackrel{2}\approx \sum_{ki}^{j-1}\left[\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\mathbf{I}-\delta \vec{\phi}_{i k}^{\wedge}\right) \cdot\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right) \cdot \Delta t-\Delta \tilde{\mathbf{R}}_{i k} \mathbf{\eta}_{k}^{a} \Delta t\right] \\ \stackrel{3}\sum_{ki}^{j-1}\left[\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right) \cdot \Delta t\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge} \cdot \delta \vec{\phi}_{i k} \cdot \Delta t-\Delta \tilde{\mathbf{R}}_{i k} \mathbf{\eta}_{k}^{a} \Delta t\right] \\ \sum_{ki}^{j-1}\left[\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right) \cdot \Delta t\right] \sum_{ki}^{j-1}\left[\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge} \cdot \delta \vec{\phi}_{i k} \cdot \Delta t-\Delta \tilde{\mathbf{R}}_{i k} \mathbf{\eta}_{k}^{a} \Delta t\right] \end{aligned} Δvijki∑j−1ΔRik⋅(a~k−bia−ηka)⋅Δt≈ki∑j−1ΔR~ik⋅Exp(−δϕ ik)⋅(a~k−bia−ηka)⋅Δt≈1ki∑j−1ΔR~ik⋅(I−δϕ ik∧)⋅(a~k−bia−ηka)⋅Δt≈2ki∑j−1[ΔR~ik⋅(I−δϕ ik∧)⋅(a~k−bia)⋅Δt−ΔR~ikηkaΔt]3ki∑j−1[ΔR~ik⋅(a~k−bia)⋅ΔtΔR~ik⋅(a~k−bia)∧⋅δϕ ik⋅Δt−ΔR~ikηkaΔt]ki∑j−1[ΔR~ik⋅(a~k−bia)⋅Δt]ki∑j−1[ΔR~ik⋅(a~k−bia)∧⋅δϕ ik⋅Δt−ΔR~ikηkaΔt]
其中1处使用了当 ϕ ⃗ \vec{\phi} ϕ 是小量时 exp ( ϕ ⃗ ∧ ) ≈ I ϕ ⃗ ∧ \exp\left(\vec{\phi}^{\wedge}\right)\approx\mathbf{I}\vec{\phi}^{\wedge} exp(ϕ ∧)≈Iϕ ∧或者 Exp ( ϕ ⃗ ) ≈ I ϕ ⃗ ∧ \operatorname{Exp}(\vec{\phi})\approx\mathbf{I}\vec{\phi}^{\wedge} Exp(ϕ )≈Iϕ ∧。
其中2处忽略高阶小项 δ ϕ ⃗ i k ∧ η k a \delta\vec{\phi}_{i k}^{\wedge}\mathbf{\eta}_{k}^{a} δϕ ik∧ηka。
其中3处使用了性质 a ∧ ⋅ b − b ∧ ⋅ a \mathbf{a}^{\wedge}\cdot\mathbf{b}-\mathbf{b}^{\wedge}\cdot\mathbf{a} a∧⋅b−b∧⋅a。
令 Δ v ~ i j ≜ ∑ k i j − 1 [ Δ R ~ i k ⋅ ( a ~ k − b i a ) ⋅ Δ t ] δ v i j ≜ ∑ k i j − 1 [ Δ R ~ i k η k a Δ t − Δ R ~ i k ⋅ ( a ~ k − b i a ) ∧ ⋅ δ ϕ ⃗ i k ⋅ Δ t ] \begin{aligned} \Delta \tilde{\mathbf{v}}_{i j} \triangleq \sum_{ki}^{j-1}\left[\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right) \cdot \Delta t\right] \\ \delta \mathbf{v}_{i j} \triangleq \sum_{ki}^{j-1}\left[\Delta \tilde{\mathbf{R}}_{i k} \mathbf{\eta}_{k}^{a} \Delta t-\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge} \cdot \delta \vec{\phi}_{i k} \cdot \Delta t\right] \end{aligned} Δv~ijδvij≜ki∑j−1[ΔR~ik⋅(a~k−bia)⋅Δt]≜ki∑j−1[ΔR~ikηkaΔt−ΔR~ik⋅(a~k−bia)∧⋅δϕ ik⋅Δt]
则有 Δ v i j ≜ Δ v ~ i j − δ v i j \Delta \mathbf{v}_{i j} \triangleq \Delta \tilde{\mathbf{v}}_{i j}-\delta \mathbf{v}_{i j} Δvij≜Δv~ij−δvij v ~ i j \tilde{\mathbf{v}}_{i j} v~ij即速度增量测量值它由IMU测量值和对零偏的估计或猜测计算得到而 δ v i j \delta\mathbf{v}_{i j} δvij即其测量噪声。 Δ p i j \Delta\mathbf{p}_{i j} Δpij真值分离
将 Δ R i j \Delta \mathbf{R}_{i j} ΔRij真值、 Δ v i j \Delta\mathbf{v}_{i j} Δvij真值整理好的式子带入 Δ p i j \Delta\mathbf{p}_{i j} Δpij真值式子中进行整理 Δ p i j ∑ k i j − 1 [ Δ v i k ⋅ Δ t 1 2 Δ R i k ⋅ ( a ~ k − b i a − η k a ) ⋅ Δ t 2 ] ≈ ∑ k i j − 1 [ ( Δ v ~ i k − δ v i k ) ⋅ Δ t 1 2 Δ R ~ i k ⋅ Exp ( − δ ϕ ⃗ i k ) ⋅ ( a ~ k − b i a − η k a ) ⋅ Δ t 2 ] ≈ 1 ∑ k i j − 1 [ ( Δ v ~ i k − δ v i k ) ⋅ Δ t 1 2 Δ R ~ i k ⋅ ( I − δ ϕ ⃗ i k ∧ ) ⋅ ( a ~ k − b i a − η k a ) ⋅ Δ t 2 ] ≈ 2 ∑ k i j − 1 [ ( Δ v ~ i k − δ v i k ) ⋅ Δ t 1 2 Δ R ~ i k ⋅ ( I − δ ϕ ⃗ i k ∧ ) ⋅ ( a ~ k − b i a ) ⋅ Δ t 2 − 1 2 Δ R ~ i k η k a Δ t 2 ] 3 ∑ k i j − 1 [ Δ v ~ i k Δ t 1 2 Δ R ~ i k ⋅ ( a ~ k − b i a ) Δ t 2 1 2 Δ R ~ i k ⋅ ( a ~ k − b i a ) ∧ δ ϕ ⃗ i k Δ t 2 − 1 2 Δ R ~ i k η k a Δ t 2 − δ v i k Δ t ] \begin{aligned} \Delta \mathbf{p}_{i j} \sum_{ki}^{j-1}\left[\Delta \mathbf{v}_{i k} \cdot \Delta t\frac{1}{2} \Delta \mathbf{R}_{i k} \cdot\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}-\boldsymbol{\eta}_{k}^{a}\right) \cdot \Delta t^{2}\right] \\ \approx \sum_{ki}^{j-1}\left[\left(\Delta \tilde{\mathbf{v}}_{i k}-\delta \mathbf{v}_{i k}\right) \cdot \Delta t\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \cdot \operatorname{Exp}\left(-\delta \vec{\phi}_{i k}\right) \cdot\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}-\boldsymbol{\eta}_{k}^{a}\right) \cdot \Delta t^{2}\right] \\ \stackrel{1}\approx \sum_{ki}^{j-1}\left[\left(\Delta \tilde{\mathbf{v}}_{i k}-\delta \mathbf{v}_{i k}\right) \cdot \Delta t\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\mathbf{I}-\delta \vec{\phi}_{i k}^{\wedge}\right) \cdot\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}-\boldsymbol{\eta}_{k}^{a}\right) \cdot \Delta t^{2}\right] \\ \stackrel{2}\approx \sum_{ki}^{j-1} \left[\left(\Delta \tilde{\mathbf{v}}_{i k}-\delta \mathbf{v}_{i k}\right) \cdot \Delta t\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\mathbf{I}-\delta \vec{\phi}_{i k}^{\wedge}\right) \cdot\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right) \cdot \Delta t^{2}-\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \boldsymbol{\eta}_{k}^{a} \Delta t^{2}\right] \\ \stackrel{3}{} \sum_{ki}^{j-1}\left[\Delta \tilde{\mathbf{v}}_{i k} \Delta t\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right) \Delta t^{2}\right. \left.\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{a}}_{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} \Delta t^{2}-\delta \mathbf{v}_{i k} \Delta t\right] \end{aligned} Δpijki∑j−1[Δvik⋅Δt21ΔRik⋅(a~k−bia−ηka)⋅Δt2]≈ki∑j−1[(Δv~ik−δvik)⋅Δt21ΔR~ik⋅Exp(−δϕ ik)⋅(a~k−bia−ηka)⋅Δt2]≈1ki∑j−1[(Δv~ik−δvik)⋅Δt21ΔR~ik⋅(I−δϕ ik∧)⋅(a~k−bia−ηka)⋅Δt2]≈2ki∑j−1[(Δv~ik−δvik)⋅Δt21ΔR~ik⋅(I−δϕ ik∧)⋅(a~k−bia)⋅Δt2−21ΔR~ikηkaΔt2]3ki∑j−1[Δv~ikΔt21ΔR~ik⋅(a~k−bia)Δt221ΔR~ik⋅(a~k−bia)∧δϕ ikΔt2−21ΔR~ikηkaΔt2−δvikΔt]
其中1处使用了当 ϕ ⃗ \vec{\phi} ϕ 是小量时 exp ( ϕ ⃗ ∧ ) ≈ I ϕ ⃗ ∧ \exp\left(\vec{\phi}^{\wedge}\right)\approx\mathbf{I}\vec{\phi}^{\wedge} exp(ϕ ∧)≈Iϕ ∧或者 Exp ( ϕ ⃗ ) ≈ I ϕ ⃗ ∧ \operatorname{Exp}(\vec{\phi})\approx\mathbf{I}\vec{\phi}^{\wedge} Exp(ϕ )≈Iϕ ∧。
其中2处忽略高阶小项 δ ϕ ⃗ i k ∧ η k a \delta\vec{\phi}_{i k}^{\wedge}\mathbf{\eta}_{k}^{a} δϕ ik∧ηka。
其中3处使用了性质 a ∧ ⋅ b − b ∧ ⋅ a \mathbf{a}^{\wedge}\cdot\mathbf{b}-\mathbf{b}^{\wedge}\cdot\mathbf{a} a∧⋅b−b∧⋅a。
令 Δ p ~ i j ≜ ∑ k i j − 1 [ Δ v ~ i k Δ t 1 2 Δ R ~ i k ⋅ ( a ~ k − b i a ) Δ t 2 ] δ p i j ≜ ∑ k i j − 1 [ δ v i k Δ t − 1 2 Δ R ~ i k ⋅ ( a ~ k − b i a ) ∧ δ ϕ ⃗ i k Δ t 2 1 2 Δ R ~ i k η k a Δ t 2 ] \begin{aligned} \Delta \tilde{\mathbf{p}}_{i j} \triangleq \sum_{ki}^{j-1}\left[\Delta \tilde{\mathbf{v}}_{i k} \Delta t\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right) \Delta t^{2}\right] \\ \delta \mathbf{p}_{i j} \triangleq \sum_{ki}^{j-1}\left[\delta \mathbf{v}_{i k} \Delta t-\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{a}}_{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} \Delta t^{2}\right] \end{aligned} Δp~ij≜ki∑j−1[Δv~ikΔt21ΔR~ik⋅(a~k−bia)Δt2]δpij≜ki∑j−1[δvikΔt−21ΔR~ik⋅(a~k−bia)∧δϕ ikΔt221ΔR~ikηkaΔt2]
则有 Δ p i j ≜ Δ p ~ i j − δ p i j \Delta \mathbf{p}_{i j} \triangleq \Delta \tilde{\mathbf{p}}_{i j}-\delta \mathbf{p}_{i j} Δpij≜Δp~ij−δpij Δ p ~ i j \Delta\tilde{\mathbf{p}}_{i j} Δp~ij即位置增量测量值它由IMU测量值和对零偏的估计得到而 δ p i j \delta\mathbf{p}_{i j} δpij即其测量噪声。
小结
上面得到PVQ增量真值和测量值的关系如下 Δ R i j ≜ Δ R ~ i j ⋅ Exp ( − δ ϕ ⃗ i j ) Δ v i j ≜ Δ v ~ i j − δ v i j Δ p i j ≜ Δ p ~ i j − δ p i j \begin{aligned} \Delta \mathbf{R}_{i j} \triangleq \Delta \tilde{\mathbf{R}}_{i j} \cdot \operatorname{Exp}\left(-\delta \vec{\phi}_{i j}\right) \\ \Delta \mathbf{v}_{i j} \triangleq \Delta \tilde{\mathbf{v}}_{i j}-\delta \mathbf{v}_{i j} \\ \Delta \mathbf{p}_{i j} \triangleq \Delta \tilde{\mathbf{p}}_{i j}-\delta \mathbf{p}_{i j} \end{aligned} ΔRij≜ΔR~ij⋅Exp(−δϕ ij)Δvij≜Δv~ij−δvijΔpij≜Δp~ij−δpij
我们也将PVQ增量噪声称为IMU预积分测量噪声PVQ增量测量值称为IMU预积分测量值。
PVQ增量噪声的分布形式
下面对PVQ增量测量噪声进行分析证明其符合高斯分布目的是给出其协方差的计算表达式令POV增量噪声为 η i j Δ ≜ [ δ ϕ ⃗ i j T δ v i j T δ p i j T ] T \mathbf{\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 Δ ∼ N ( 0 9 × 1 , Σ i j ) \boldsymbol{\eta}_{i j}^{\Delta}\sim N\left(\mathbf{0}_{9 \times 1}, \boldsymbol{\Sigma}_{i j}\right) ηijΔ∼N(09×1,Σij) 。由于 η i j Δ \boldsymbol{\eta}_{i j}^{\Delta} ηijΔ是 δ ϕ ⃗ i j T \delta\vec{\phi}_{i j}^{T} δϕ ijT、 δ v i j T \delta\mathbf{v}_{i j}^{T} δvijT、 δ p i j T \delta\mathbf{p}_{i j}^{T} δpijT 的线性组合下面分别分析这三个噪声项的分布形式。 δ ϕ ⃗ i j T \delta\vec{\phi}_{i j}^{T} δϕ ijT的分布形式
根据上面分离出的噪声 δ ϕ ⃗ i j \delta\vec{\phi}_{i j} δϕ ij δ ϕ ⃗ i j − log ( ∏ k i j − 1 Exp ( − Δ R ~ k 1 j T ⋅ J r k ⋅ η k g Δ t ) ) \delta \vec{\phi}_{i j}-\log \left(\prod_{ki}^{j-1} \operatorname{Exp}\left(-\Delta \tilde{\mathbf{R}}_{k1 j}^{T} \cdot \mathbf{J}_{r}^{k} \cdot \mathbf{\eta}_{k}^{g} \Delta t\right)\right) δϕ ij−log(ki∏j−1Exp(−ΔR~k1jT⋅Jrk⋅ηkgΔt))
令 ξ k Δ R ~ k 1 j T ⋅ J r k ⋅ η k g Δ t \boldsymbol{\xi}_{k}\Delta \tilde{\mathbf{R}}_{k1 j}^{T} \cdot \mathbf{J}_{r}^{k} \cdot \mathbf{\eta}_{k}^{g} \Delta t ξkΔR~k1jT⋅Jrk⋅ηkgΔt
由于 η k g \mathbf{\eta}_{k}^{g} ηkg 是小量因此 ξ k \boldsymbol{\xi}_{k} ξk也是小量于是 J r ( ξ k ) ≈ I \mathbf{J}_{r}\left(\xi_{k}\right)\approx\mathbf{I} Jr(ξk)≈I、 J r − 1 ( ξ k ) ≈ I \mathbf{J}_{r}^{-1}\left(\xi_{k}\right)\approx\mathbf{I} Jr−1(ξk)≈I并利用BCH公式的近似形式 log ( Exp ( ϕ ⃗ ) ⋅ Exp ( δ ϕ ⃗ ) ) ϕ ⃗ J r − 1 ( ϕ ⃗ ) ⋅ δ ϕ ⃗ \log (\operatorname{Exp}(\vec{\phi}) \cdot \operatorname{Exp}(\delta \vec{\phi}))\vec{\phi}\mathbf{J}_{r}^{-1}(\vec{\phi}) \cdot \delta \vec{\phi} log(Exp(ϕ )⋅Exp(δϕ ))ϕ Jr−1(ϕ )⋅δϕ
因此 δ ϕ ⃗ 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_{ki}^{j-1} \operatorname{Exp}\left(-\xi_{k}\right)\right) \\ -\log \left(\operatorname{Exp}\left(-\xi_{i}\right) \prod_{ki1}^{j-1} \operatorname{Exp}\left(-\xi_{k}\right)\right) \\ \approx-\left(-\xi_{i}\mathbf{I} \cdot \log \left(\prod_{ki1}^{j-1} \operatorname{Exp}\left(-\xi_{k}\right)\right)\right)\\ \xi_{i}-\log \left(\prod_{ki1}^{j-1} \operatorname{Exp}\left(-\xi_{k}\right)\right) \\ \xi_{i}-\log \left(\operatorname{Exp}\left(-\xi_{i1}\right) \prod_{ki2}^{j-1} \operatorname{Exp}\left(-\xi_{k}\right)\right) \\ \approx \xi_{i}\xi_{i1}-\log \left(\prod_{ki2}^{j-1} \operatorname{Exp}\left(-\xi_{k}\right)\right) \\ \approx \sum_{ki}^{j-1} \xi_{k} \end{aligned} δϕ ij−log(ki∏j−1Exp(−ξk))−log(Exp(−ξi)ki1∏j−1Exp(−ξk))≈−(−ξiI⋅log(ki1∏j−1Exp(−ξk)))ξi−log(ki1∏j−1Exp(−ξk))ξi−log(Exp(−ξi1)ki2∏j−1Exp(−ξk))≈ξiξi1−log(ki2∏j−1Exp(−ξk))≈ki∑j−1ξk
即 δ ϕ ⃗ i j ≈ ∑ k i j − 1 Δ R ~ k 1 j T J r k η k g Δ t \delta \vec{\phi}_{i j} \approx \sum_{ki}^{j-1} \Delta \tilde{\mathbf{R}}_{k1 j}^{T} \mathbf{J}_{r}^{k} \mathbf{\eta}_{k}^{g} \Delta t δϕ ij≈ki∑j−1ΔR~k1jTJrkηkgΔt
由于 Δ R ~ k 1 j T \Delta\tilde{\mathbf{R}}_{k1 j}^{T} ΔR~k1jT、 J r k \mathbf{J}_{r}^{k} Jrk、 Δ t \Delta t Δt都是已知量而 η k g \mathbf{\eta}_{k}^{g} ηkg 是零均值高斯噪声因此 δ ϕ ⃗ i j \delta\vec{\phi}_{i j} δϕ ij 的一阶近似也是零均值高斯噪声。 δ v i j T \delta\mathbf{v}_{i j}^{T} δvijT 的分布形式
由于 δ ϕ ⃗ i j \delta\vec{\phi}_{i j} δϕ ij 近似拥有了高斯噪声的形式且 η k a \boldsymbol{\eta}_{k}^{a} ηka 也是零均值高斯噪声根据 δ v i j T \delta\mathbf{v}_{i j}^{T} δvijT 的表达式可知其也拥有高斯分布的形式。 δ p i j T \delta\mathbf{p}_{i j}^{T} δpijT 的分布形式
类似于 δ v i j T \delta\mathbf{v}_{i j}^{T} δvijT δ p i j T \delta\mathbf{p}_{i j}^{T} δpijT也拥有高斯分布的形式。
PVQ增量噪声递推
下面推导预积分测量噪声的递推形式即 η i j − 1 Δ → η i j Δ \mathbf{\eta}_{i j-1}^{\Delta}\rightarrow\mathbf{\eta}_{i j}^{\Delta} ηij−1Δ→ηijΔ及其协方差 Σ i j \boldsymbol{\Sigma}_{i j} Σij 的递推形式 Σ i j − 1 → Σ i j \boldsymbol{\Sigma}_{i j-1}\rightarrow\boldsymbol{\Sigma}_{i j} Σij−1→Σij 。
下面依次推导 δ ϕ ⃗ i j − 1 → δ ϕ ⃗ i j \delta\vec{\phi}_{i j-1}\rightarrow\delta\vec{\phi}_{i j} δϕ ij−1→δϕ ij、 δ v i j − 1 → δ v i j \delta\mathbf{v}_{i j-1}\rightarrow\delta\mathbf{v}_{i j} δvij−1→δvij、 δ p i j − 1 → δ p i j \delta\mathbf{p}_{i j-1}\rightarrow\delta\mathbf{p}_{i j} δpij−1→δpij。 δ ϕ ⃗ i j − 1 → δ ϕ ⃗ i j \delta\vec{\phi}_{i j-1}\rightarrow\delta\vec{\phi}_{i j} δϕ ij−1→δϕ ij递推 δ ϕ ⃗ i j ∑ k i j − 1 Δ R ~ k 1 j T J r k η k g Δ t ∑ k i j − 2 Δ R ~ k 1 j T J r k η k g Δ t Δ R ~ j j T J r j − 1 η j − 1 g Δ t 1 ∑ k i j − 2 ( Δ R ~ k 1 j − 1 Δ R ~ j − 1 j ) T J r k η k g Δ t J r j − 1 η j − 1 g Δ t Δ R ~ j j − 1 ∑ k i j − 2 Δ R ~ k 1 j − 1 T J r k η k g Δ t J r j − 1 η j − 1 g Δ t Δ R ~ j j − 1 δ ϕ ⃗ i j − 1 J r j − 1 η j − 1 g Δ t \begin{aligned} \delta \vec{\phi}_{i j} \sum_{ki}^{j-1} \Delta \tilde{\mathbf{R}}_{k1 j}^{T} \mathbf{J}_{r}^{k} \mathbf{\eta}_{k}^{g} \Delta t \\ \sum_{ki}^{j-2} \Delta \tilde{\mathbf{R}}_{k1 j}^{T} \mathbf{J}_{r}^{k} \boldsymbol{\eta}_{k}^{\operatorname{g}} \Delta t\Delta \tilde{\mathbf{R}}_{j j}^{T} \mathbf{J}_{r}^{j-1} \boldsymbol{\eta}_{j-1}^{g} \Delta t \\ \stackrel{1}{} \sum_{ki}^{j-2}\left(\Delta \tilde{\mathbf{R}}_{k1 j-1} \Delta \tilde{\mathbf{R}}_{j-1 j}\right)^{T} \mathbf{J}_{r}^{k} \boldsymbol{\eta}_{k}^{g} \Delta t\mathbf{J}_{r}^{j-1} \boldsymbol{\eta}_{j-1}^{g} \Delta t \\ \Delta \tilde{\mathbf{R}}_{j j-1} \sum_{ki}^{j-2} \Delta \tilde{\mathbf{R}}_{k1 j-1}^{T} \mathbf{J}_{r}^{k} \boldsymbol{\eta}_{k}^{g} \Delta t\mathbf{J}_{r}^{j-1} \boldsymbol{\eta}_{j-1}^{g} \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} \Delta t \end{aligned} δϕ ijki∑j−1ΔR~k1jTJrkηkgΔtki∑j−2ΔR~k1jTJrkηkgΔtΔR~jjTJrj−1ηj−1gΔt1ki∑j−2(ΔR~k1j−1ΔR~j−1j)TJrkηkgΔtJrj−1ηj−1gΔtΔR~jj−1ki∑j−2ΔR~k1j−1TJrkηkgΔtJrj−1ηj−1gΔtΔR~jj−1δϕ ij−1Jrj−1ηj−1gΔt
其中1处利用了 Δ R ~ j j T I \Delta\tilde{\mathbf{R}}_{j j}^{T}\mathbf{I} ΔR~jjTI 以及 Δ R ~ l m Δ R ~ m n Δ R ~ l n \Delta\tilde{\mathbf{R}}_{l m}\Delta\tilde{\mathbf{R}}_{m n}\Delta\tilde{\mathbf{R}}_{l n} ΔR~lmΔR~mnΔR~ln 的性质推导过程中进行了一些变形。 δ v i j − 1 → δ v i j \delta\mathbf{v}_{i j-1}\rightarrow\delta\mathbf{v}_{i j} δvij−1→δvij递推 δ v i j ∑ k i j − 1 [ Δ R ~ i k η k α d Δ t − Δ R ~ i k ⋅ ( a ~ k − b i a ) ∧ ⋅ δ ϕ ⃗ i k ⋅ Δ t ] ∑ k i j − 2 [ Δ R ~ i k η k a Δ t − Δ R ~ i k ⋅ ( a ~ k − b i a ) ∧ ⋅ δ ϕ ⃗ i k ⋅ Δ t ] Δ R ~ i j − 1 η j − 1 a Δ t − Δ R ~ i j − 1 ⋅ ( a ~ j − 1 − b i a ) ∧ ⋅ δ ϕ ⃗ i j − 1 ⋅ Δ t δ v i j − 1 Δ R ~ i j − 1 η j − 1 a Δ t − Δ R ~ i j − 1 ⋅ ( a ~ j − 1 − b i a ) ∧ ⋅ δ ϕ ⃗ i j − 1 ⋅ Δ t \begin{aligned} \delta \mathbf{v}_{i j} \sum_{ki}^{j-1}\left[\Delta \tilde{\mathbf{R}}_{i k} \boldsymbol{\eta}_{k}^{\alpha d} \Delta t-\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge} \cdot \delta \vec{\phi}_{i k} \cdot \Delta t\right] \\ \sum_{ki}^{j-2}\left[\Delta \tilde{\mathbf{R}}_{i k} \boldsymbol{\eta}_{k}^{a} \Delta t-\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge} \cdot \delta \vec{\phi}_{i k} \cdot \Delta t\right] \Delta \tilde{\mathbf{R}}_{i j-1} \boldsymbol{\eta}_{j-1}^{a} \Delta t-\Delta \tilde{\mathbf{R}}_{i j-1} \cdot\left(\tilde{\mathbf{a}}_{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} \Delta t-\Delta \tilde{\mathbf{R}}_{i j-1} \cdot\left(\tilde{\mathbf{a}}_{j-1}-\mathbf{b}_{i}^{a}\right)^{\wedge} \cdot \delta \vec{\phi}_{i j-1} \cdot \Delta t \end{aligned} δvijki∑j−1[ΔR~ikηkαdΔt−ΔR~ik⋅(a~k−bia)∧⋅δϕ ik⋅Δt]ki∑j−2[ΔR~ikηkaΔt−ΔR~ik⋅(a~k−bia)∧⋅δϕ ik⋅Δt]ΔR~ij−1ηj−1aΔt−ΔR~ij−1⋅(a~j−1−bia)∧⋅δϕ ij−1⋅Δtδvij−1ΔR~ij−1ηj−1aΔt−ΔR~ij−1⋅(a~j−1−bia)∧⋅δϕ ij−1⋅Δt
直接进行加项拆分即可完成推导。 δ p i j − 1 → δ p i j \delta\mathbf{p}_{i j-1}\rightarrow\delta\mathbf{p}_{i j} δpij−1→δpij递推 δ p i j ∑ k i j − 1 [ δ v i k Δ t − 1 2 Δ R ~ i k ⋅ ( a ~ k − b i a ) ∧ δ ϕ ⃗ i k Δ t 2 1 2 Δ R ~ i k η k a Δ t 2 ] δ p i j − 1 δ v i j − 1 Δ t − 1 2 Δ R ~ i j − 1 ⋅ ( a ~ j − 1 − b i a ) ∧ δ ϕ ⃗ i j − 1 Δ t 2 1 2 Δ R ~ i j − 1 η j − 1 a Δ t 2 \begin{aligned} \delta \mathbf{p}_{i j} \sum_{ki}^{j-1}\left[\delta \mathbf{v}_{i k} \Delta t-\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{a}}_{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} \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{a}}_{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} \Delta t^{2} \end{aligned} δpijki∑j−1[δvikΔt−21ΔR~ik⋅(a~k−bia)∧δϕ ikΔt221ΔR~ikηkaΔt2]δpij−1δvij−1Δt−21ΔR~ij−1⋅(a~j−1−bia)∧δϕ ij−1Δt221ΔR~ij−1ηj−1aΔt2
直接进行加项拆分即可完成推导。
小结
已知定义 η i j Δ ≜ [ δ ϕ ⃗ i j T δ v i j T δ p i j T ] T \mathbf{\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
令 η k [ ( η k g ) T ( η k a ) T ] T \boldsymbol{\eta}_{k}^{}\left[\left(\boldsymbol{\eta}_{k}^{g}\right)^{T} \quad\left(\mathbf{\eta}_{k}^{a}\right)^{T}\right]^{T} ηk[(ηkg)T(ηka)T]T
综上可得 η i j Δ \boldsymbol{\eta}_{i j}^{\Delta} ηijΔ 的递推形式如下 η i j Δ [ Δ R ~ j j − 1 0 0 − Δ R ~ i j − 1 ⋅ ( a ~ j − 1 − b i a ) ∧ Δ t I 0 − 1 2 Δ R ~ i j − 1 ⋅ ( a ~ j − 1 − b i a ) ∧ Δ t 2 Δ t I I ] η i j − 1 Δ [ J r j − 1 Δ t 0 0 Δ R ~ i j − 1 Δ t 0 1 2 Δ R ~ i j − 1 Δ t 2 ] η j − 1 \begin{aligned} \boldsymbol{\eta}_{i j}^{\Delta}\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{a}}_{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{a}}_{j-1}-\mathbf{b}_{i}^{a}\right)^{\wedge} \Delta t^{2} \Delta t \mathbf{I} \mathbf{I} \end{array}\right] \mathbf{\eta}_{i j-1}^{\Delta} \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] \boldsymbol{\eta}_{j-1}^{} \end{aligned} ηijΔ ΔR~jj−1−ΔR~ij−1⋅(a~j−1−bia)∧Δt−21ΔR~ij−1⋅(a~j−1−bia)∧Δt20IΔtI00I ηij−1Δ Jrj−1Δt000ΔR~ij−1Δt21ΔR~ij−1Δt2 ηj−1
令 A j − 1 [ Δ R ~ j j − 1 0 0 − Δ R ~ i j − 1 ⋅ ( a ~ j − 1 − b i a ) ∧ Δ t I 0 − 1 2 Δ R ~ i j − 1 ⋅ ( a ~ j − 1 − b i a ) ∧ Δ t 2 Δ t I I ] B j − 1 [ J r j − 1 Δ t 0 0 Δ R ~ i j − 1 Δ t 0 1 2 Δ R ~ i j − 1 Δ t 2 ] \begin{aligned} \mathbf{A}_{j-1}\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{a}}_{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{a}}_{j-1}-\mathbf{b}_{i}^{a}\right)^{\wedge} \Delta t^{2} \Delta t \mathbf{I} \mathbf{I} \end{array}\right] \\ \mathbf{B}_{j-1}\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] \end{aligned} Aj−1 ΔR~jj−1−ΔR~ij−1⋅(a~j−1−bia)∧Δt−21ΔR~ij−1⋅(a~j−1−bia)∧Δt20IΔtI00I Bj−1 Jrj−1Δt000ΔR~ij−1Δt21ΔR~ij−1Δt2
则有PVQ增量噪声的递推形式 η i j Δ A j − 1 η i j − 1 Δ B j − 1 η j − 1 \mathbf{\eta}_{i j}^{\Delta}\mathbf{A}_{j-1} \mathbf{\eta}_{i j-1}^{\Delta}\mathbf{B}_{j-1} \mathbf{\eta}_{j-1}^{} ηijΔAj−1ηij−1ΔBj−1ηj−1
PVQ增量噪声的协方差矩阵就有了如下的递推计算形式 Σ i j A j − 1 Σ i j − 1 A j − 1 T B j − 1 Σ η B j − 1 T \boldsymbol{\Sigma}_{i j}\mathbf{A}_{j-1} \boldsymbol{\Sigma}_{i j-1} \mathbf{A}_{j-1}^{T}\mathbf{B}_{j-1} \mathbf{\Sigma}_{\boldsymbol{\eta}} \mathbf{B}_{j-1}^{T} ΣijAj−1Σij−1Aj−1TBj−1ΣηBj−1T
从形式上看PVQ增量噪声的协方差的递推形式类似于卡尔曼滤波中的状态变量协方差的预测方程其中的 Q \mathbf{Q} Q 就相当于 Σ η \Sigma_{\eta} Ση 在每个递推周期都固定的加上这样一个常量噪声表示从当前状态转移到下一个状态的过程中存在各种噪声总是会引入新的误差 P ‾ F P F ⊤ Q \overline{\mathbf{P}}\mathbf{F P F}^{\top}\mathbf{Q} PFPF⊤Q
PVQ增量噪声的协方差矩阵即噪声分布将用来计算信息矩阵在优化框架中起到平衡权重的作用。在实际应用中首先要求协方差矩阵的逆矩阵相当于取了协方差的倒数方差越大权重越小反之权重越大然后再将逆矩阵转成信息矩阵与残差相乘起到调节残差比例的作用。
关于噪声的内容到此为止接下来讨论零偏的问题。 相关阅读
IMU预积分的理解和推导简明预积分推导如何理解IMU以及其预积分
