本章开始将会开始对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 }) R=Exp(ϕ)=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 k=i k=i到 k = j − 1 k=j-1 k=j−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_{k=i}^{j-1} \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{k}^{g}-\boldsymbol{\eta}_{k}^{g}\right) \cdot \Delta t\right) Rj=Ri⋅k=i∏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_{k=i}^{j-1} \mathbf{R}_{k} \cdot\left(\tilde{\mathbf{a}}_{k}-\mathbf{b}_{k}^{a}-\mathbf{\eta}_{k}^{a}\right) \cdot \Delta t vj=vi+g⋅Δtij+k=i∑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_{k=i}^{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} pj=pi+k=i∑j−1[vk⋅Δt+21g⋅Δt2+21Rk⋅(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_{k=i}^{j-1}\Delta t=(j-i)\Delta t Δtij=k=i∑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_{k=i}^{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≜RiTRj=k=i∏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_{k=i}^{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)=k=i∑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_{k=i}^{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)=k=i∑j−1[Δvik⋅Δt+21Δ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}_{i+1}^{g}=\cdots=\mathbf{b}_{j}^{g} big=bi+1g=⋯=bjg以及 b i a = b i + 1 a = ⋯ = b j a \mathbf{b}_{i}^{a}=\mathbf{b}_{i+1}^{a}=\cdots=\mathbf{b}_{j}^{a} bia=bi+1a=⋯=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_{k=i}^{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_{k=i}^{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}}_{i+1}-\mathbf{b}_{i}^{g}\right) \Delta t\right) \cdot \operatorname{Exp}\left(-\mathbf{J}_{r}^{i+1} \cdot \mathbf{\eta}_{i+1}^{g} \Delta t\right) ...\\ &= \Delta \tilde{\mathbf{R}}_{i,i+1} \cdot \operatorname{Exp}\left(-\mathbf{J}_{r}^i \cdot \mathbf{\eta}_{i}^{g} \Delta t\right) \cdot \Delta \tilde{\mathbf{R}}_{i+1,i+2} \cdot \operatorname{Exp}\left(-\mathbf{J}_{r}^{i+1} \cdot \mathbf{\eta}_{i+1}^{g} \Delta t\right) \cdot \Delta \tilde{\mathbf{R}}_{i+2,i+3} ...\\ &\stackrel{3}= \Delta \tilde{\mathbf{R}}_{i,i+1} \cdot \Delta \tilde{\mathbf{R}}_{i+1,i+2} \cdot \operatorname{Exp}\left(-\Delta \tilde{\mathbf{R}}_{i+1,i+2}^T \cdot \mathbf{J}_{r}^i \cdot \mathbf{\eta}_{i}^{g} \Delta t\right) \cdot \operatorname{Exp}\left(-\mathbf{J}_{r}^{i+1} \cdot \mathbf{\eta}_{i+1}^{g} \Delta t\right) \cdot \Delta \tilde{\mathbf{R}}_{i+2,i+3}...\\ &\stackrel{4}= \Delta \tilde{\mathbf{R}}_{i,j} \cdot \operatorname{Exp}\left(-\Delta \tilde{\mathbf{R}}_{i+1,j}^T \cdot \mathbf{J}_{r}^i \cdot \mathbf{\eta}_{i}^{g} \Delta t\right) \cdot \operatorname{Exp}\left(-\Delta \tilde{\mathbf{R}}_{i+2,j}^T \cdot \mathbf{J}_{r}^{i+1} \cdot \mathbf{\eta}_{i+1}^{g} \Delta t\right)...\\ & = \Delta \tilde{\mathbf{R}}_{i j} \cdot \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} \Delta t\right) \end{aligned} ΔRij=k=i∏j−1Exp((ω~k−big)Δt−ηkgΔt)≈1k=i∏j−1{Exp((ω~k−big)Δt)⋅Exp(−Jrk⋅ηkgΔt)}=2Exp((ω~i−big)Δt)⋅Exp(−Jri⋅ηigΔt)⋅Exp((ω~i+1−big)Δt)⋅Exp(−Jri+1⋅ηi+1gΔt)...=ΔR~i,i+1⋅Exp(−Jri⋅ηigΔt)⋅ΔR~i+1,i+2⋅Exp(−Jri+1⋅ηi+1gΔt)⋅ΔR~i+2,i+3...=3ΔR~i,i+1⋅ΔR~i+1,i+2⋅Exp(−ΔR~i+1,i+2T⋅Jri⋅ηigΔt)⋅Exp(−Jri+1⋅ηi+1gΔt)⋅ΔR~i+2,i+3...=4ΔR~i,j⋅Exp(−ΔR~i+1,jT⋅Jri⋅ηigΔt)⋅Exp(−ΔR~i+2,jT⋅Jri+1⋅ηi+1gΔt)...=ΔR~ij⋅k=i∏j−1Exp(−ΔR~k+1jT⋅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(ϕ)⋅R=R⋅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) Jrk=Jr((ω~k−big)Δt)
Δ R ~ i j = ∏ k = i j − 1 Exp ( ( ω ~ k − b i g ) Δ t ) \Delta \tilde{\mathbf{R}}_{i j}=\prod_{k=i}^{j-1} \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{i}^{g}\right) \Delta t\right) ΔR~ij=k=i∏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_{k=i}^{j-1} \operatorname{Exp}\left(-\Delta \tilde{\mathbf{R}}_{k+1 j}^{T} \cdot \mathbf{J}_{r}^{k} \cdot \mathbf{\eta}_{k}^{g} \Delta t\right) Exp(−δϕij)=k=i∏j−1Exp(−ΔR~k+1jT⋅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_{k=i}^{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_{k=i}^{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_{k=i}^{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_{k=i}^{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_{k=i}^{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_{k=i}^{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_{k=i}^{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} Δvij=k=i∑j−1ΔRik⋅(a~k−bia−ηka)⋅Δt≈k=i∑j−1ΔR~ik⋅Exp(−δϕik)⋅(a~k−bia−ηka)⋅Δt≈1k=i∑j−1ΔR~ik⋅(I−δϕik∧)⋅(a~k−bia−ηka)⋅Δt≈2k=i∑j−1[ΔR~ik⋅(I−δϕik∧)⋅(a~k−bia)⋅Δt−ΔR~ikηkaΔt]=3k=i∑j−1[ΔR~ik⋅(a~k−bia)⋅Δt+ΔR~ik⋅(a~k−bia)∧⋅δϕik⋅Δt−ΔR~ikηkaΔt]=k=i∑j−1[ΔR~ik⋅(a~k−bia)⋅Δt]+k=i∑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_{k=i}^{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_{k=i}^{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≜k=i∑j−1[ΔR~ik⋅(a~k−bia)⋅Δt]≜k=i∑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_{k=i}^{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_{k=i}^{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_{k=i}^{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_{k=i}^{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_{k=i}^{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} Δpij=k=i∑j−1[Δvik⋅Δt+21ΔRik⋅(a~k−bia−ηka)⋅Δt2]≈k=i∑j−1[(Δv~ik−δvik)⋅Δt+21ΔR~ik⋅Exp(−δϕik)⋅(a~k−bia−ηka)⋅Δt2]≈1k=i∑j−1[(Δv~ik−δvik)⋅Δt+21ΔR~ik⋅(I−δϕik∧)⋅(a~k−bia−ηka)⋅Δt2]≈2k=i∑j−1[(Δv~ik−δvik)⋅Δt+21ΔR~ik⋅(I−δϕik∧)⋅(a~k−bia)⋅Δt2−21ΔR~ikηkaΔt2]=3k=i∑j−1[Δv~ikΔt+21ΔR~ik⋅(a~k−bia)Δt2+21Δ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_{k=i}^{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_{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{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≜k=i∑j−1[Δv~ikΔt+21ΔR~ik⋅(a~k−bia)Δt2]δpij≜k=i∑j−1[δvikΔt−21ΔR~ik⋅(a~k−bia)∧δϕikΔt2+21Δ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_{k=i}^{j-1} \operatorname{Exp}\left(-\Delta \tilde{\mathbf{R}}_{k+1 j}^{T} \cdot \mathbf{J}_{r}^{k} \cdot \mathbf{\eta}_{k}^{g} \Delta t\right)\right) δϕij=−log(k=i∏j−1Exp(−ΔR~k+1jT⋅Jrk⋅ηkgΔt))
令：
ξ k = Δ R ~ k + 1 j T ⋅ J r k ⋅ η k g Δ t \boldsymbol{\xi}_{k}=\Delta \tilde{\mathbf{R}}_{k+1 j}^{T} \cdot \mathbf{J}_{r}^{k} \cdot \mathbf{\eta}_{k}^{g} \Delta t ξk=ΔR~k+1jT⋅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_{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 \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
即：
δ ϕ ⃗ i j ≈ ∑ k = i j − 1 Δ R ~ k + 1 j T J r k η k g Δ t \delta \vec{\phi}_{i j} \approx \sum_{k=i}^{j-1} \Delta \tilde{\mathbf{R}}_{k+1 j}^{T} \mathbf{J}_{r}^{k} \mathbf{\eta}_{k}^{g} \Delta t δϕij≈k=i∑j−1ΔR~k+1jTJrkηkgΔt
由于 Δ R ~ k + 1 j T \Delta\tilde{\mathbf{R}}_{k+1 j}^{T} ΔR~k+1jT、 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_{k=i}^{j-1} \Delta \tilde{\mathbf{R}}_{k+1 j}^{T} \mathbf{J}_{r}^{k} \mathbf{\eta}_{k}^{g} \Delta t \\ &=\sum_{k=i}^{j-2} \Delta \tilde{\mathbf{R}}_{k+1 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_{k=i}^{j-2}\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} \Delta t+\mathbf{J}_{r}^{j-1} \boldsymbol{\eta}_{j-1}^{g} \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} \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} δϕij=k=i∑j−1ΔR~k+1jTJrkηkgΔt=k=i∑j−2ΔR~k+1jTJrkηkgΔt+ΔR~jjTJrj−1ηj−1gΔt=1k=i∑j−2(ΔR~k+1j−1ΔR~j−1j)TJrkηkgΔt+Jrj−1ηj−1gΔt=ΔR~jj−1k=i∑j−2ΔR~k+1j−1TJrkηkgΔt+Jrj−1ηj−1gΔt=ΔR~jj−1δϕij−1+Jrj−1ηj−1gΔt
其中1处利用了 Δ R ~ j j T = I \Delta\tilde{\mathbf{R}}_{j j}^{T}=\mathbf{I} ΔR~jjT=I 以及 Δ 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_{k=i}^{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_{k=i}^{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} δvij===k=i∑j−1[ΔR~ikηkαdΔt−ΔR~ik⋅(a~k−bia)∧⋅δϕik⋅Δt]k=i∑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_{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{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} δpij==k=i∑j−1[δvikΔt−21ΔR~ik⋅(a~k−bia)∧δϕikΔt2+21ΔR~ikηkaΔt2]δpij−1+δvij−1Δt−21ΔR~ij−1⋅(a~j−1−bia)∧δϕij−1Δt2+21Δ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} Σij=Aj−1Σij−1Aj−1T+Bj−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} P=FPF⊤+Q
PVQ增量噪声的协方差矩阵（即噪声分布）将用来计算信息矩阵，在优化框架中起到平衡权重的作用。在实际应用中首先要求协方差矩阵的逆矩阵，相当于取了协方差的倒数，方差越大权重越小，反之权重越大，然后再将逆矩阵转成信息矩阵，与残差相乘，起到调节残差比例的作用。
关于噪声的内容到此为止，接下来讨论零偏的问题。
相关阅读
- IMU预积分的理解和推导
- 简明预积分推导
- 如何理解IMU以及其预积分
![[Kubernetes] 多调度器(1/3)：如何编译scheduler，以默认调度器 kube-scheduler为例](http://pic.xiahunao.cn/[Kubernetes] 多调度器(1/3)：如何编译scheduler，以默认调度器 kube-scheduler为例)
