流形上的預積分

文章目錄

• 一、預積分的由來
• 二、相關理論
• 三、初步
• a.黎曼幾何概念
• b.SO(3)中不確定性的描述
• c.流形上的高斯牛頓法
• 四、最大後驗視覺慣導狀態估計
• 五、IMU模型和運動積分
• 六、流形上的IMU預積分
• A. *Preintegrated IMU Measurements*
• (1) $\Delta {\bf R}_{ij}$ΔRij​項
• (2) $\Delta {\bf v}_{ij}$Δvij​項
• (3) $\Delta{\bf p}_{ij}$Δpij​項
• B. *Noise Propagation*
• B1. _Iterative Noise Propagation_
• C. _Incorporating Bias Updates_
• D. _Preintegrated IMU Factors_
• D1. _Jacobians of Residual Errors_
• 七、無結構化視覺因子

參考文獻

• https://blog.csdn.net/qq_26682225/article/details/72649858
• On-Manifold Preintegration for Real-TimeVisual-Inertial Odometry
• 【泡泡機器人原創專欄】IMU預積分總結與公式推導（一）～（四）

一、預積分的由來

來協調imu數據和圖像之間的關係。通過重新參數化[^1]來把關鍵幀之間的IMU數據積分成相對運動的約束，避免初始條件變化 [^2]造成的重複積分, 預積分的值並不是真實的測量值.

---

預積分與因子圖結合, 增量式因子圖

二、相關理論

• 基於濾波的方法能夠快速的推斷當前的狀態量，但是精度會隨着線性化誤差的累積而惡化。
• 基於優化的完全平滑方法精度高一些，但計算量很大。
• 固定滯後的平滑方法（滑動窗）是一個折中。

---

採用了structless model(邊際化)，這樣可以不用延遲視覺信息的處理，同時可以多次線性化視覺測量模型。具體代碼的實現整合在了gtsam4.0中

---

• 參加估計的pose number：

full smoothers: 所有的pose.

fixed-lag: 滑動窗口內的pose

filter: 最近的pose

• 代表不確定性的方法：

EKF: 協方差矩陣

信息濾波[^3]和smoothers: 信息矩陣

• 測量模型被線性化的次數：

standard EKF :1

smoothing appoach : 多次

---

fixed-lag和filter的方式相比對外點有更強的魯棒性，fixed-lag在邊際化的會使H矩陣變得稠密，爲了保持稀疏性，需要丟掉一些測量點。此外兩者都需要採用first estimate jacobian來線性化，來保持系統能觀性不變，否則會引入錯誤的信息。

1. SLAM中的First-Estimates Jacobian。

對於一個系統，Observability性質（能觀性），決定了這個系統在進行狀態估計時，哪些自由度是可以被估計出來的。並且其能觀性是不受估計方法（Closed-form 方法、EKF、或者Nonlinear Optimization等等）改變的。能觀性通過Observability Matrix（能觀性矩陣）體現，系統Unobservable的狀態維數是這個矩陣零空間的維數。

比如，單目純視覺SLAM裏，尺度和6DOF的絕對位姿——總共7DOF，都是無法被估計。可以通過固定某一幀來確定6個自由度，但是尺度無法確定。

再比如Visual-Inertial系統裏，在運動激勵充分（足夠多軸向有足夠大的加速度/角速度）的情況下，尺度、滾轉/俯仰角是可以被估計的，只剩下絕對偏航角、絕對位置無法獲得，也就是說對於Visual-Inertial系統在合適的運動模式下Unobservable的維數是4。磁力計可以確定偏航角，加上固定某一幀可以確定相對位置。

不同的線性化狀態點計算得到的係數矩陣（雅克比矩陣）會導致Observability Matrix（能觀性矩陣）的零空間維數出現不一致（inconsistency），導致自由度不一樣出現偏差。爲了修正這個問題，提出了First Estimate Jacobian。用相同的狀態點進行線性化計算就不會有這個問題了，具體來說，位姿采用propagated值而非updated值，landmark使用第一次估計值。First Estimate就是這個直觀含義，即採用estimation from the first time。

理解不夠透徹：可參考文獻：

[1] Kelly, J., & Sukhatme, G. S. (2011). Visual-inertial sensor fusion: Localization, mapping and sensor-to-sensor self-calibration. The International Journal of Robotics Research, 30(1), 56-79

[3] Strasdat, H., Montiel, J. M. M., & Davison, A. J. (2010). Scale drift-aware large scale monocular SLAM. Robotics: Science and Systems VI.

[4] Huang, G. P., Mourikis, A. I., & Roumeliotis, S. I. (2009). A first-estimates Jacobian EKF for improving SLAM consistency. In Experimental Robotics(pp. 373-382). Springer Berlin Heidelberg.作者：

[8] Hermann, R., & Krener, A. J. (1977). Nonlinear controllability and observability. IEEE Transactions on automatic control, 22(5), 728-740.

---

預積分理論的貢獻

• 使得測量與絕對位姿解耦
• 利用重力使得俯仰和橫滾變得可觀
• 與視覺結合使得bias可觀

---

三、初步

a.黎曼幾何概念

SO3羣定義：
${ SO(3) =\lbrace {\bf R} \in {\Bbb R} :{\bf R^TR} = {\bf I}, det({\bf R}) = 1 \rbrace. }$SO(3)={R∈R:RTR=I,det(R)=1}.
他也代表着一個光滑的流形(manifold), 流形的切空間(tan)表示爲李代數 $\mathcal SO(3)$SO(3), 它與一個3維向量的3*3的反對稱矩陣(向量加^表示)一致.
${\omega ^\wedge = \begin{bmatrix} \omega_1 \\ \omega_2 \\ \omega_3 \end{bmatrix}^\wedge = \begin{bmatrix} 0 && -\omega_3 && \omega_2 \\ \omega_3 && 0 && -\omega_1 \\ -\omega_2 && \omega_1 && 0 \end{bmatrix} \in \mathcal{SO} (3)}.$ω∧=⎣⎡​ω1​ω2​ω3​​⎦⎤​∧=⎣⎡​0ω3​−ω2​​​−ω3​0ω1​​​ω2​−ω1​0​⎦⎤​∈SO(3).
他們之間的指數映射和對數映射如下：
$\begin{aligned} Exp \ &{:}\quad \Bbb{R}^3 &{\rightarrow} \quad & SO(3) &\ ; &\quad\bf{\phi} &\rightarrow \quad &exp(\phi^\wedge) \\ Log \ &{:}\quad SO(3) &{\rightarrow} \quad & \Bbb{R}^3 &\ ; &\quad\bf{R} &\rightarrow \quad &log( {\bf R} ) \end{aligned}$Exp Log ​:R3:SO(3)​→→​SO(3)R3​ ; ;​ϕR​→→​exp(ϕ∧)log(R)​
$Exp$Exp映射:
$Exp({\mathbf \phi})=I+\frac{sin(\|{\mathbf \phi}\|)}{\|{\mathbf \phi}\|}{\mathbf \phi}^\wedge+\frac{1-cos(\|{\mathbf \phi}\|)}{\|{\mathbf \phi}\|^2}({\bf \phi}^\wedge)^2$Exp(ϕ)=I+∥ϕ∥sin(∥ϕ∥)​ϕ∧+∥ϕ∥21−cos(∥ϕ∥)​(ϕ∧)2
$Log$Log映射:
$log(R)=\frac{\phi \cdot ({\bf R-R}^T)}{2sin(\phi)} ,\quad 其中 \quad旋轉角 \phi=cos^{-1}(\frac{tr(R)-1}{2})$log(R)=2sin(ϕ)ϕ⋅(R−RT)​,其中旋轉角ϕ=cos−1(2tr(R)−1​)
李羣的乘法並不符合正常指數的運算法則，即不等於李代數的和，因此當變化量是微小量時使用BCH公式近似模型得到一階近似：
$Exp({\bf\phi}\ +\ \delta{\bf\phi} ) \approx Exp({\bf\phi}) \, Exp(J_r(\phi)\delta{\bf\phi})$Exp(ϕ + δϕ)≈Exp(ϕ)Exp(Jr​(ϕ)δϕ)
其中
$J_r(\phi) = {\bf I} - \frac {1 - \cos(\parallel\phi\parallel)} {\parallel\phi\parallel ^2}\phi^\wedge \ + \ \frac {\mid\mid\phi\mid\mid -\sin(\mid\mid\phi\mid\mid )}{\mid\mid\phi\mid\mid ^3}(\phi^\wedge)^2$Jr​(ϕ)=I−∥ϕ∥21−cos(∥ϕ∥)​ϕ∧ + ∣∣ϕ∣∣3∣∣ϕ∣∣−sin(∣∣ϕ∣∣)​(ϕ∧)2
同理反之，乘法變加法也成立。

圖中， $J_r(\phi)$Jr​(ϕ)將切線空間中的累加增量與左側的乘法增量相關聯。
$R \ Exp({\bf\phi}) \ R^T\ = \ exp(R{\bf\phi}^\wedge R^T) \ = \ Exp(R{\bf\phi})$R Exp(ϕ) RT = exp(Rϕ∧RT) = Exp(Rϕ)

$\iff Exp({\bf\phi}) \ R \ = \ R Exp(R^T{\bf\phi}).$⟺Exp(ϕ) R = RExp(RTϕ).

注意： 這裏的 $Exp$Exp和 $exp$exp是有區別的，大寫的不需要^這個。

---

b.SO(3)中不確定性的描述

由前面知識可知：
$\tilde{R} = R\ Exp(\epsilon), \qquad \epsilon \sim N(0,\ \Sigma)$R~=R Exp(ϵ),ϵ∼N(0, Σ)
其中， $R$R是不包含噪聲的， $\epsilon$ϵ是一個正態分佈的小擾動.

我們對高斯擾動(3維高斯公式)進行積分:
$\int_{\R^3} p(\epsilon)\ d\epsilon = \int_{\R^3} \alpha e^{-\frac{1}{2}\parallel\epsilon\parallel^2_\Sigma }d \epsilon=1,$∫R3​p(ϵ) dϵ=∫R3​αe−21​∥ϵ∥Σ2​dϵ=1,
其中 $\alpha = 1/ \sqrt{(2\pi)^3 det(\Sigma)}$α=1/(2π)3det(Σ) ​, $\parallel \epsilon\parallel^2_\Sigma = \epsilon^T\Sigma^{-1}\epsilon$∥ϵ∥Σ2​=ϵTΣ−1ϵ是馬氏距離的平方.

當 $\parallel\epsilon\parallel < \pi$∥ϵ∥<π時, 根據式(8)可以推出 $\epsilon = Log(R^{-1}\tilde{R})$ϵ=Log(R−1R~)代入式(9)
$\int_{SO(3)} \beta(\tilde{R}) \ e^{-\frac{1}{2}\parallel Log(R^{-1}\tilde{R})\parallel^2_\Sigma }d \tilde{R} = 1,$∫SO(3)​β(R~) e−21​∥Log(R−1R~)∥Σ2​dR~=1,
$\beta(\tilde{R})$β(R~)是一個歸一化因子, 他等於 $\beta(\tilde{R})=\alpha/\mid det(J(\tilde{R}))\mid$β(R~)=α/∣det(J(R~))∣, 由於積分變量的變化 $J(\tilde{R})\doteq J_r(Log(R^{-1}\tilde{R}))$J(R~)≐Jr​(Log(R−1R~))

關於積分變量的推導：
$\begin{aligned} \delta R&= Log( \frac {Exp(\epsilon+\delta\epsilon)}{Exp(\epsilon)}) \\ &=Log(\frac {Exp(\epsilon) Exp(J_r\delta\epsilon)}{Exp(\epsilon)}) \\ &=J_r\delta\epsilon \end{aligned}$δR​=Log(Exp(ϵ)Exp(ϵ+δϵ)​)=Log(Exp(ϵ)Exp(ϵ)Exp(Jr​δϵ)​)=Jr​δϵ​
在SO(3)上的高斯擾動:
$p(\tilde{R}) = \beta(\tilde{R}) \ e^{-\frac{1}{2}\parallel Log(R^{-1}\tilde{R})\parallel^2_\Sigma }$p(R~)=β(R~) e−21​∥Log(R−1R~)∥Σ2​
$\epsilon$ϵ是小的協方差矩陣則, $\beta\approx\alpha$β≈α, 即 $J(\tilde{R})=1$J(R~)=1, 並且 $\tilde{R}$R~接近R, 求其最大似然(log):
${\mathcal L}(R) = \frac{1}{2}\parallel Log(R^{-1}\tilde{R})\parallel^2_\Sigma + const = \frac{1}{2}\parallel Log(\tilde{R}^{-1}{R})\parallel^2_\Sigma + const$L(R)=21​∥Log(R−1R~)∥Σ2​+const=21​∥Log(R~−1R)∥Σ2​+const
其中const是 $Log(\beta)$Log(β).

他的幾何意義是SO(3)上的距離的平方, 不確定性權重爲協方差的逆 $\Sigma^{-1}$Σ−1.

c.流形上的高斯牛頓法

歐幾里得空間的高斯牛頓法是迭代優化目標函數的二次近似（通常非凸），求解中通常簡化爲一系列的線性方程。

當變量屬於流形時
$\min_{x\in\mathcal{M}}\ f(x)$x∈Mmin​ f(x)
爲了簡化，我們只考慮一個變量時。

這裏我們不能直接近似爲 $x$x的二次函數。因爲，

• 會使參數增多（9×3個）會導致欠定。
• 這麼做會使結果不再是流形 $\mathcal M$M上的了，不滿足SO(3)定義。

通常的做法是定義一個映射 $\mathcal{R}_x$Rx​, 它是由李代數(tan 空間)上的 $\delta x$δx到 $x \in \mathcal{M}$x∈M流形上附近值的一個映射.如下:
$\min_{x\in\mathcal{M}}\ f(x) \Rightarrow \min_{\delta x \in \R^n} f(\mathcal{R}(\delta x)).$x∈Mmin​ f(x)⇒δx∈Rnmin​f(R(δx)).
這種重新參數化叫做lifting.。這樣就可以在歐幾里得空間(也就是tan空間)進行優化，粗略地說當前估計定義在tan空間，而附近的增量是歐幾里得空間。

在高斯牛頓框架下，求解當前估計的代價函數平方，得到最小時的李代數 $\delta x^*$δx∗(tan空間),最終更新流形上的估計值
$\hat x \gets {\mathcal R}_{\hat x}(\delta x^*)$x^←Rx^​(δx∗)
這種方法是誤差狀態模型的基礎和統一推廣,通常是用於航空航天文獻中的濾波方法.

本文中使用以下定義這種retraction(縮放?)
${\mathcal R}_R(\phi)=\rm R \ Exp(\delta \phi), \quad \delta\phi \in\R^3$RR​(ϕ)=R Exp(δϕ),δϕ∈R3

${\mathcal R}_T(\delta\phi,\delta {\bf p})=({\rm R \ Exp(\delta\phi)}, {\bf p}+{\rm R} \delta {\bf p} ), \quad \begin{bmatrix}\delta \phi &\delta{\bf p}\end{bmatrix} \in \R^6$RT​(δϕ,δp)=(R Exp(δϕ),p+Rδp),[δϕ​δp​]∈R6

---

四、最大後驗視覺慣導狀態估計

IMU的座標系與機體座標系一致,相機與機體系之間的變換關係已知.

系統狀態變量爲姿態,位置,速度,偏差:
${\bf x}_i \doteq [{\bf R}_i,{\bf p}_i,{\bf v}_i,{\bf b}_i]$xi​≐[Ri​,pi​,vi​,bi​]
${\mathcal K}_k$Kk​表示到時間k所有關鍵幀的序列,我們估計所有關鍵幀的狀態:
${\mathcal K}_k\doteq \lbrace {\rm X}_i \rbrace_{i\in{\mathcal K}_k}$Kk​≐{Xi​}i∈Kk​​
測量的關鍵幀圖像 ${\mathcal C}_i$Ci​包含許多路標點 $l$l, 因此有觀測值 ${\bf z}_{il}$zil​. ${\mathcal I}_{ij}$Iij​表示兩關鍵幀之間的IMU測量值. 因此測量可以表示爲:
${\mathcal Z}_k \doteq \lbrace{\mathcal C}_i ,{\mathcal I}_{ij} \rbrace _{(i, j)\in{\mathcal K}_k}$Zk​≐{Ci​,Iij​}(i,j)∈Kk​​
${\mathcal X}_k$Xk​的後驗概率:
$p({\mathcal X}_k | {\mathcal Z _k} )\propto p({\mathcal X}_0)p({\mathcal Z}_k | {\mathcal X }_k ) \overset {(a)}= p({\mathcal X}_0) \prod _{(i,j)\in{\mathcal K_k}}p({\mathcal C_i, I_{ij}|}{\mathcal X_k}) \\ \overset {(b)}=p({\mathcal X_0}) \prod _{(i,j)\in{\mathcal K_k}}p({\mathcal I_{ij}|x_i, x_j}) \prod_{i \in {\mathcal K_k}} \prod_{l \in {\mathcal C_i}}p(z_{il}|x_i)$p(Xk​∣Zk​)∝p(X0​)p(Zk​∣Xk​)=(a)p(X0​)(i,j)∈Kk​∏​p(Ci​,Iij​∣Xk​)=(b)p(X0​)(i,j)∈Kk​∏​p(Iij​∣xi​,xj​)i∈Kk​∏​l∈Ci​∏​p(zil​∣xi​)
使用因子圖來求解.

使用-log形式進行MAP估計, 可以寫成殘差像的和, 通俗說就是當前狀態下預測值與測量值之間的不匹配度.
$\begin{aligned} {\mathcal X}_k^* &\doteq arg \min _{{\mathcal X}_k} -\log_e p({\mathcal X}_k|{\mathcal Z}_k) \\ &=\arg \min _{{\mathcal X}_k}\|{\bf r}_0\|^2_{\Sigma_0}+\sum_{(i, j)\in{\mathcal K}_k}\|{\bf r}_{{\mathcal I}_{i,j}}\|^2_{\Sigma_{i,j}}+\sum_{i \in{\mathcal K}_k} \sum_{l\in{\mathcal C_i}}\|{\bf r}_{{\mathcal C}_{il}}\|^2_{\Sigma_{\mathcal C}} \end{aligned}$Xk∗​​≐argXk​min​−loge​p(Xk​∣Zk​)=argXk​min​∥r0​∥Σ0​2​+(i,j)∈Kk​∑​∥rIi,j​​∥Σi,j​2​+i∈Kk​∑​l∈Ci​∑​∥rCil​​∥ΣC​2​​

---

五、IMU模型和運動積分

陀螺儀測量模型:
${_B}\tilde{\bf \omega}_{WB}(t)={_B}{\omega}_{WB}(t)+{\bf b}^{g}(t)+{\bf \eta}^g(t)$B​ω~WB​(t)=B​ωWB​(t)+bg(t)+ηg(t)
加速度計測量模型:
$_B \tilde {\bf a}(t)={\rm R_{WB}^{T}}(t)(_{W}{\bf a}(t)-_{W}{\bf g})+{\bf b}^a(t)+{\bf \eta}^a(t)$B​a~(t)=RWBT​(t)(W​a(t)−W​g)+ba(t)+ηa(t)
其中下標B表示機體座標系, W表示世界座標系(靜止的), WB表示B繫到W系轉換. ${\bf b}$b爲緩慢變化的bias; $\eta$η表示白噪聲.

運動模型:
$\dot{R_{\rm WB}}={R_{\rm WB}} _{\rm B}{\bf \omega}_{\rm WB}^{\wedge}, \quad _{\rm W}\dot{\bf v}=_{\rm W}{\bf a}, \quad _{\rm W}\dot{\bf p}=_{\rm W}{\bf v}$RWB​˙​=RWB​B​ωWB∧​,W​v˙=W​a,W​p˙​=W​v

在時間段 $[t,t+\Delta t]$[t,t+Δt]上使用歐拉積分
$\begin{aligned} &{\rm R_{WB}}(t+\Delta t)={\rm R_{WB}}(t){\rm Exp}(_{\rm B}{\bf \omega}_{\rm WB}(t)\Delta t) \\ &_{\rm W}{\bf v}(t+\Delta t)=_{\rm W}{\bf v}(t)+_{\rm W}{\bf a}(t)\Delta t \\ &_{\rm W}{\bf p}(t+\Delta t)=_{\rm W}{\bf p}(t)+_{\rm W}{\bf v}(t)\Delta t+\frac{1}{2} {_{\rm W}{\bf a}(t)}\Delta t^2 \end{aligned} \tag {28}$​RWB​(t+Δt)=RWB​(t)Exp(B​ωWB​(t)Δt)W​v(t+Δt)=W​v(t)+W​a(t)ΔtW​p(t+Δt)=W​p(t)+W​v(t)Δt+21​W​a(t)Δt2​(28)
將(25)(26)的測量模型公式代入得到:
$\begin{aligned} &{\rm R}(t+\Delta t)={\rm R}(t){\rm Exp}((\tilde{\omega}(t)-{\bf b}^{g}(t)-{\bf \eta}^{gd}(t))\Delta t) \\ &{\bf v}(t+\Delta t)={\bf v}(t)+{\bf g}\Delta t +{\rm R}(t)(\tilde {\bf a}(t)-{\bf b}^a(t)-{\bf \eta}^{ad}(t))\Delta t \\ &{\bf p}(t+\Delta t)={\bf p}(t)+{\bf v}(t)\Delta t+\frac{1}{2} {\bf g}\Delta t^2+\frac{1}{2}{\rm R}(t) (\tilde{\bf a}(t)-{\bf b}^a(t)-{\bf \eta}^{ad}(t))\Delta t^2 \end{aligned}\tag {29}$​R(t+Δt)=R(t)Exp((ω~(t)−bg(t)−ηgd(t))Δt)v(t+Δt)=v(t)+gΔt+R(t)(a~(t)−ba(t)−ηad(t))Δtp(t+Δt)=p(t)+v(t)Δt+21​gΔt2+21​R(t)(a~(t)−ba(t)−ηad(t))Δt2​(29)
這裏簡化了符號的表示. 公式中我們假設 ${\rm R}(t)$R(t)是恆定不變的, 這樣會導致產生誤差, 但當IMU頻率較高時, 這個影響會大大減小. 當使用的IMU的頻率較小時, 需要使用更高階的數值積分方法.

其中的白噪聲離散形式協方差爲原噪聲協方差的除以時間間隔 $\Delta t$Δt.

---

六、流形上的IMU預積分

IMU預積分的初衷，是希望借鑑純視覺SLAM中圖優化的思想，將幀與幀之間IMU相對測量信息轉換爲約束節點（載體位姿）的邊參與到優化框架中。

• IMU預積分理論最大的貢獻是對這些IMU相對測量進行處理，使得它與絕對位姿解耦（或者只需要線性運算就可以進行校正），從而大大提高優化速度。
• 這種優化架構還使得加計測量中不受待見的重力變成一個有利條件——重力的存在將使整個系統對絕對姿態（指相對水平地理座標系的俯仰角和橫滾角，不包括真航向）可觀。要知道純視覺VO或者SLAM是完全無法得到絕對姿態的。
• 兩種傳感器融合可以利用冗餘測量（例如兩種方式都可以求取相對位姿）來抑制累積誤差。
• IMU和視覺這兩種不同源的測量，也使得IMU的bias可觀，從而可以在優化中被有效估計。
• 純單目視覺缺乏絕對尺度的問題，也可以由慣性信息的引入而得以解決。

將兩關鍵幀 $i$i和 $j$j之間的綜合成一個複合的測量值, 叫preintegrated IMU measurement, 它約束連續關鍵幀之間的運動.

兩個關鍵幀之間的關係如下:
$\begin{aligned} &{\bf R}_j={\bf R}_i \prod_{k=i}^{j-1}{\rm Exp((\tilde \omega_k-b_k^g-\eta_k^{gd})\Delta t)}, \\ &{\bf v}_j={\bf v}_i+{\bf g}\Delta t_{ij}+\sum_{k=i}^{j-1}{\bf R}_k(\tilde{\bf a}_k-{\bf b}_k^a-\eta_k^{ad})\Delta t \\ &{\bf p}_j={\bf p}_i+\sum_{k=i}^{j-1}[{\bf v}_k \Delta t+\frac{1}{2}{\bf g}\Delta t^2+\frac{1}{2}{\bf R}_k(\tilde{\bf a}_k-{\bf b}_k^a-\eta_k^{ad})\Delta t^2] \end{aligned}\tag {30}$​Rj​=Ri​k=i∏j−1​Exp((ω~k​−bkg​−ηkgd​)Δt),vj​=vi​+gΔtij​+k=i∑j−1​Rk​(a~k​−bka​−ηkad​)Δtpj​=pi​+k=i∑j−1​[vk​Δt+21​gΔt2+21​Rk​(a~k​−bka​−ηkad​)Δt2]​(30)
從該公式(30)可以看出當初始值改變, 如 ${\bf R}_i$Ri​改變會導致每一個公式都需要重新計算. 爲了解決這個缺陷, 給出不受線性化點影響的增量表達式:
$\begin{aligned} &\Delta {\bf R}_{ij}={\bf R}_i^T{\bf R}_j= \prod_{k=i}^{j-1}{\rm Exp((\tilde \omega_k-b_k^g-\eta_k^{gd})\Delta t)}, \\ &\Delta {\bf v}_{ij}={\bf R}_i^T({\bf v}_j-{\bf v}_i-{\bf g}\Delta t_{ij})=\sum_{k=i}^{j-1}\Delta{\bf R}_{ik}(\tilde{\bf a}_k-{\bf b}_k^a-\eta_k^{ad})\Delta t \\ &\Delta {\bf p}_{ij} ={\bf R}_i^T({\bf p}_j-{\bf p}_i-{\bf v}_i \Delta t_{ij}-\frac{1}{2}{\bf g}\Delta t_{ij}^2)\\ & \qquad \ =\sum_{k=i}^{j-1}[\Delta{\bf v}_{ik} \Delta t+\frac{1}{2}\Delta{\bf R}_{ik}(\tilde{\bf a}_k-{\bf b}_k^a-\eta_k^{ad})\Delta t^2] \end{aligned}\tag {31}$​ΔRij​=RiT​Rj​=k=i∏j−1​Exp((ω~k​−bkg​−ηkgd​)Δt),Δvij​=RiT​(vj​−vi​−gΔtij​)=k=i∑j−1​ΔRik​(a~k​−bka​−ηkad​)ΔtΔpij​=RiT​(pj​−pi​−vi​Δtij​−21​gΔtij2​) =k=i∑j−1​[Δvik​Δt+21​ΔRik​(a~k​−bka​−ηkad​)Δt2]​(31)
最後的式子應用了等差數列求和公式推導, 證明如下:

這裏的速度和位置 $\Delta$Δ值並不能代表真正的物理變化, 但卻使得它與 $i$i時刻狀態和重力獨立, 能夠直接根據慣性測量值計算出來. 這些值仍是Bias的函數, 假設Bias在兩關鍵幀之間保持不變.

A. Preintegrated IMU Measurements

(1) $\Delta {\bf R}_{ij}$ΔRij​項

公式中包含的測量噪聲使得MAP(最大後驗估計)無法應用, 我們對公式進行變換, 把噪聲獨立出來, 使得log似然更容易獲得. 假設 $i$i時刻bias已知且不變. 使用公式(6)和公式(9)將連乘展開後, 從第二個開始使用公式（8）進行兩兩合併, 之後再進行兩兩合併, 直到前面只有第一個Exp的連乘, 後面只有第二個Exp的複合連乘)_進行變換:
$\begin{aligned} \Delta {\bf R}_{ij} &\overset{eq(6)} \simeq \prod_{k=i}^{j-1}\ [{\rm Exp((\tilde \omega_k-b_i^g)\Delta t)}{\rm Exp}(-{\rm J}_r^k \eta_k^{gd} \Delta t)] \\ &\overset{eq(9)} = \Delta \tilde {\bf R}_{ij}\prod_{k=i}^{j-1}{\rm Exp}(-\Delta \tilde {\bf R}_{k+1j}^T {\rm J}_r^k \eta_k^{gd} \Delta t) \\ &\doteq \Delta \tilde {\bf R}_{ij}{\rm Exp}(-\delta \phi_{ij}) \end{aligned}\tag {32}$ΔRij​​≃eq(6)k=i∏j−1​ [Exp((ω~k​−big​)Δt)Exp(−Jrk​ηkgd​Δt)]=eq(9)ΔR~ij​k=i∏j−1​Exp(−ΔR~k+1jT​Jrk​ηkgd​Δt)≐ΔR~ij​Exp(−δϕij​)​(32)
其中:
${\rm J}_r^k \doteq {\rm J}_r^k((\tilde \omega_k-b_i^g)\Delta t) \\ \Delta \tilde {\bf R}_{ij}=\prod_{k=i}^{j-1}\ {\rm Exp}((\tilde \omega_k-b_i^g)\Delta t) \tag {33}$Jrk​≐Jrk​((ω~k​−big​)Δt)ΔR~ij​=k=i∏j−1​ Exp((ω~k​−big​)Δt)(33)
$\delta \phi_{ij}$δϕij​定義爲噪聲.

總結:
$\Delta {\bf R}_{ij}\doteq \Delta \tilde {\bf R}_{ij}{\rm Exp}(- \delta \phi_{ij}) \\ \Delta \tilde {\bf R}_{ij}=\prod_{k=i}^{j-1}\ {\rm Exp}((\tilde \omega_k-b_i^g)\Delta t) \\ {\rm Exp}(- \delta \phi_{ij}) =\prod_{k=i}^{j-1}{\rm Exp}(-\Delta \tilde {\bf R}_{k+1j}^T {\rm J}_r^k \eta_k^{gd} \Delta t) \tag {34}$ΔRij​≐ΔR~ij​Exp(−δϕij​)ΔR~ij​=k=i∏j−1​ Exp((ω~k​−big​)Δt)Exp(−δϕij​)=k=i∏j−1​Exp(−ΔR~k+1jT​Jrk​ηkgd​Δt)(34)

(2) $\Delta {\bf v}_{ij}$Δvij​項

將式(32)代入(31)的速度預積分公式中, 對Exp進行一階近似, 並且忽略高次微小量.得到:

$\begin{aligned} \Delta {\bf v}_{ij} &= \sum_{k=i}^{j-1}\Delta \tilde {\bf R}_{ik}{\rm Exp}(- \delta \phi_{ik})(\tilde{\bf a}_k-{\bf b}_k^a-\eta_k^{ad})\Delta t \\ &= \sum_{k=i}^{j-1}[ \Delta \tilde {\bf R}_{ik}({\bf I}-\delta \phi_{ik}^\wedge)(\tilde{\bf a}_k-{\bf b}_k^a)\Delta t - \Delta \tilde {\bf R}_{ik}\eta_k^{ad}\Delta t ] \\ &=\sum_{k=i}^{j-1}[ \Delta \tilde {\bf R}_{ik}(\tilde{\bf a}_k-{\bf b}_k^a)\Delta t ]+\sum_{k=i}^{j-1}[ \Delta \tilde {\bf R}_{ik}(\tilde{\bf a}_k-{\bf b}_k^a)^\wedge\delta \phi_{ik}\Delta t- \Delta \tilde {\bf R}_{ik}\eta_k^{ad}\Delta t ] \end{aligned} \tag {35}$Δvij​​=k=i∑j−1​ΔR~ik​Exp(−δϕik​)(a~k​−bka​−ηkad​)Δt=k=i∑j−1​[ΔR~ik​(I−δϕik∧​)(a~k​−bka​)Δt−ΔR~ik​ηkad​Δt]=k=i∑j−1​[ΔR~ik​(a~k​−bka​)Δt]+k=i∑j−1​[ΔR~ik​(a~k​−bka​)∧δϕik​Δt−ΔR~ik​ηkad​Δt]​(35)
總結:
$\Delta {\bf v}_{ij} \triangleq \Delta \tilde {\bf v}_{ij}-\delta {\bf v}_{ij} \\ \Delta \tilde {\bf v}_{ij}=\sum_{k=i}^{j-1}[ \Delta \tilde {\bf R}_{ik}(\tilde{\bf a}_k-{\bf b}_k^a)\Delta t ] \\ \delta {\bf v}_{ij} = \sum_{k=i}^{j-1}[ \Delta \tilde {\bf R}_{ik}\eta_k^{ad}\Delta t -\Delta \tilde {\bf R}_{ik}(\tilde{\bf a}_k-{\bf b}_k^a)^\wedge\delta \phi_{ik}\Delta t ] \tag {36}$Δvij​≜Δv~ij​−δvij​Δv~ij​=k=i∑j−1​[ΔR~ik​(a~k​−bka​)Δt]δvij​=k=i∑j−1​[ΔR~ik​ηkad​Δt−ΔR~ik​(a~k​−bka​)∧δϕik​Δt](36)

(3) $\Delta{\bf p}_{ij}$Δpij​項

同上(34)和(36)式代入(31)中得到:
$\begin{aligned} \Delta {\bf p}_{ij}&=\sum_{k=i}^{j-1}[ ( \Delta \tilde {\bf v}_{ik}-\delta {\bf v}_{ik})\Delta t + \frac{1}{2} \Delta \tilde {\bf R}_{ik}({\bf I}-\delta \phi_{ik}^\wedge)(\tilde{\bf a}_k-{\bf b}_k^a)\Delta t^2 -\frac{1}{2} \Delta \tilde {\bf R}_{ik}\eta_k^{ad}\Delta t^2 ]\\ &=\sum_{k=i}^{j-1}[ \Delta \tilde {\bf v}_{ik}\Delta t+\frac{1}{2}\Delta \tilde {\bf R}_{ik}(\tilde{\bf a}_k-{\bf b}_k^a)\Delta t^2 ] \\ & \quad + \sum_{k=i}^{j-1}[\frac{1}{2}\Delta \tilde {\bf R}_{ik}(\tilde{\bf a}_k-{\bf b}_k^a)^\wedge \delta\phi_{ik}\Delta t^2 -\frac{1}{2} \Delta \tilde {\bf R}_{ik}\eta_k^{ad}\Delta t^2-\delta {\bf v}_{ik}\Delta t] \end{aligned} \tag{37}$Δpij​​=k=i∑j−1​[(Δv~ik​−δvik​)Δt+21​ΔR~ik​(I−δϕik∧​)(a~k​−bka​)Δt2−21​ΔR~ik​ηkad​Δt2]=k=i∑j−1​[Δv~ik​Δt+21​ΔR~ik​(a~k​−bka​)Δt2]+k=i∑j−1​[21​ΔR~ik​(a~k​−bka​)∧δϕik​Δt2−21​ΔR~ik​ηkad​Δt2−δvik​Δt]​(37)
總結:
$\Delta {\bf p}_{ij} \triangleq \Delta \tilde {\bf p}_{ij}-\delta {\bf p}_{ij} \\ \Delta \tilde {\bf p}_{ij} =\sum_{k=i}^{j-1}[ \Delta \tilde {\bf v}_{ik}\Delta t+\frac{1}{2}\Delta \tilde {\bf R}_{ik}(\tilde{\bf a}_k-{\bf b}_k^a)\Delta t^2 ] \\ \delta {\bf p}_{ij} = \sum_{k=i}^{j-1}[\frac{1}{2} \Delta \tilde {\bf R}_{ik}\eta_k^{ad}\Delta t^2 -\frac{1}{2}\Delta \tilde {\bf R}_{ik}(\tilde{\bf a}_k-{\bf b}_k^a)^\wedge \delta\phi_{ik}\Delta t^2 + \delta {\bf v}_{ik}\Delta t] \tag{38}$Δpij​≜Δp~​ij​−δpij​Δp~​ij​=k=i∑j−1​[Δv~ik​Δt+21​ΔR~ik​(a~k​−bka​)Δt2]δpij​=k=i∑j−1​[21​ΔR~ik​ηkad​Δt2−21​ΔR~ik​(a~k​−bka​)∧δϕik​Δt2+δvik​Δt](38)
公式(34)(36)(38)代入公式(31)得到預積分測量模型，公式（39）左側爲預積分測量值(由IMU測量值和Bias的猜測估計值組成), 右側爲真值+預積分測量噪聲.
$\Delta \tilde {\bf R}_{ij} ={\bf R}_i^{\rm T}{\bf R}_j{\rm Exp}(\delta\phi_{ij}) \\ \Delta \tilde{\bf v}_{ij}={\bf R}_i^T({\bf v}_j-{\bf v}_i-{\bf g}\Delta t_{ij})+\delta{\bf v_{ij}} \\ \Delta \tilde{\bf p}_{ij}={\bf R}_i^T({\bf p}_j-{\bf p}_i-{\bf v}_i \Delta t_{ij}-\frac{1}{2}{\bf g}\Delta t_{ij}^2)+\delta{\bf p}_{ij} \tag{39}$ΔR~ij​=RiT​Rj​Exp(δϕij​)Δv~ij​=RiT​(vj​−vi​−gΔtij​)+δvij​Δp~​ij​=RiT​(pj​−pi​−vi​Δtij​−21​gΔtij2​)+δpij​(39)

B. Noise Propagation

希望使噪聲符合高斯分佈, 定義噪聲向量:
${\bf \eta}_{ij}^\Delta \triangleq[\delta\phi_{ij}^{\rm T}, \ \delta{\bf v}_{ij}^{\rm T}, \ \delta{\bf p}_{ij}^{\rm T}]^{\rm T} \sim {\mathcal N}(0_{9\times1},\Sigma_{ij}) \tag{40}$ηijΔ​≜[δϕijT​, δvijT​, δpijT​]T∼N(09×1​,Σij​)(40)
對旋轉量預積分噪聲 $\delta \phi _{ij}$δϕij​兩端取-Log得到:
$\delta \phi_{ij}=-{\rm Log}(\Pi_{k=i}^{j-1}{\rm Exp}(-\Delta \tilde{\bf R}_{k+1j}^{\rm T}{\rm J}^k_r\eta^{gd}_k\Delta t)) \tag{41}$δϕij​=−Log(Πk=ij−1​Exp(−ΔR~k+1jT​Jrk​ηkgd​Δt))(41)
其中的 $\eta_k^{gd}$ηkgd​爲微小量, 利用公式
$\rm Log( Exp(\phi)\ Exp(\delta \phi))=\phi+J_r^{-1}(\phi) \delta \phi \tag{42}$Log(Exp(ϕ) Exp(δϕ))=ϕ+Jr−1​(ϕ)δϕ(42)
得到
$\delta \phi_{ij} \simeq \Sigma_{k=i}^{j-1}\Delta\tilde{\bf R}_{k+1j}^{\rm T}{\rm J}^k_r\boldsymbol{\eta}^{gd}_k\Delta t \tag{43}$δϕij​≃Σk=ij−1​ΔR~k+1jT​Jrk​ηkgd​Δt(43)
證明:

由公式(43)可知, $\delta \phi _{ij}$δϕij​是 $\boldsymbol{\eta}^{gd}_k$ηkgd​線性組合, 因此也是零均值的高斯分佈.

對於 $\delta{\bf v}_{ij}, \ \delta{\bf p}_{ij}$δvij​, δpij​, 根據(36)(38)的增量項公式可知它們是 $\delta \phi _{ij}$δϕij​和 $\boldsymbol{\eta}^{ad}_k$ηkad​的線性組合, 因此也符合零均值的高斯分佈.

B1. Iterative Noise Propagation

我們可以通過遞推的形式求解 $\boldsymbol{\eta}_{ij}^{\Delta}$ηijΔ​和其協方差 $\boldsymbol{\Sigma}_{ij}$Σij​.
$\begin{aligned} \delta \phi_{ij}&=\Delta \tilde{\bf R}_{jj-1}\delta\phi_{ij-1}+{\bf J}_r^{j-1}\boldsymbol{\eta}_{j-1}^{gd}\Delta t \\ \delta{\bf v}_{ij}&= \delta {\bf v}_{ij-1}+ \Delta \tilde {\bf R}_{ij-1}\eta_{j-1}^{ad}\Delta t -\Delta \tilde {\bf R}_{ij-1}(\tilde{\bf a}_{j-1}-{\bf b}_i^a)^\wedge\delta \phi_{ij-1}\Delta t \\ \delta{\bf p}_{ij}&=\delta{\bf p}_{ij-1}+\delta {\bf v}_{ij-1}\Delta t+\frac{1}{2} \Delta \tilde {\bf R}_{ij-1}\boldsymbol{\eta}_{j-1}^{ad}\Delta t^2 -\frac{1}{2}\Delta \tilde {\bf R}_{ij-1}(\tilde{\bf a}_{j-1}-{\bf b}_i^a)^\wedge \delta\phi_{ij-1}\Delta t^2 \end{aligned} \tag{44}$δϕij​δvij​δpij​​=ΔR~jj−1​δϕij−1​+Jrj−1​ηj−1gd​Δt=δvij−1​+ΔR~ij−1​ηj−1ad​Δt−ΔR~ij−1​(a~j−1​−bia​)∧δϕij−1​Δt=δpij−1​+δvij−1​Δt+21​ΔR~ij−1​ηj−1ad​Δt2−21​ΔR~ij−1​(a~j−1​−bia​)∧δϕij−1​Δt2​(44)
其中第一個公式證明如下, 後兩個公式即拆出一項即可.

定義IMU的測量噪聲向量爲
$\boldsymbol{\eta}^d_{k}=[\boldsymbol{\eta}^{gd}_{k}, \ \boldsymbol{\eta}^{ad}_{k}] \tag{45}$ηkd​=[ηkgd​, ηkad​](45)
將式(44)寫成矩陣形式:
$\boldsymbol{\eta}^\Delta_{ij}={\bf A}_{j-1}\boldsymbol{\eta}^\Delta_{ij-1}+{\bf B}_{j-1}\boldsymbol{\eta}^d_{j-1} \tag{46}$ηijΔ​=Aj−1​ηij−1Δ​+Bj−1​ηj−1d​(46)
其中
${\bf A}_{j-1}=\begin{bmatrix} \Delta\tilde{\bf R}_{jj-1} &0 &0 \\ -\Delta \tilde {\bf R}_{ij-1}(\tilde{\bf a}_{j-1}-{\bf b}_i^a)^\wedge\Delta t & {\bf I} & 0 \\ -\frac{1}{2}\Delta \tilde {\bf R}_{ij-1}(\tilde{\bf a}_{j-1}-{\bf b}_i^a)^\wedge\Delta t^2 &\Delta t{\bf I} &{\bf I} \end{bmatrix} \tag{47}$Aj−1​=⎣⎡​ΔR~jj−1​−ΔR~ij−1​(a~j−1​−bia​)∧Δt−21​ΔR~ij−1​(a~j−1​−bia​)∧Δt2​0IΔtI​00I​⎦⎤​(47)

${\bf B}_{j-1} = \begin{bmatrix} {\bf J}_r^{j-1}\Delta t & 0 \\ 0 &\Delta \tilde {\bf R}_{ij-1}\Delta t \\ 0& \frac{1}{2}\Delta \tilde {\bf R}_{ij-1}\Delta t^2 \end{bmatrix} \tag{48}$Bj−1​=⎣⎡​Jrj−1​Δt00​0ΔR~ij−1​Δt21​ΔR~ij−1​Δt2​⎦⎤​(48)

據此可以求得預積分測量噪聲的協方差矩陣:
$\Sigma_{ij}={\bf A}_{j-1}\Sigma_{ij-1}{\bf A}_{j-1}^T+{\bf B}_{j-1}\Sigma_{\eta}{\bf B}^T_{j-1} \tag{49}$Σij​=Aj−1​Σij−1​Aj−1T​+Bj−1​Ση​Bj−1T​(49)

C. Incorporating Bias Updates

之前的計算都是在bias恆定的假設下進行的，但這是不現實的。重新計算預積分值會耗費大量的資源，因此使用一階展開式來計算在bias增加一個小量時的預積分值的更新。
$\Delta \tilde {\bf R}_{ij} ({\bf \hat b}^g_i )\approx\Delta \tilde {\bf R}_{ij} (\overline {\bf b}^g_i) \cdot {\rm Exp} \left( \frac{\partial \Delta \overline {\bf R}_{ij} }{\partial{\bf \overline b}^g} \delta{\bf b}^g_i \right) \\ \Delta \tilde {\bf v}_{ij} ({\bf \hat b}^g_i ,{\bf \hat b}^a_i) \approx\Delta \tilde {\bf v}_{ij} ({\bf \overline b}^g_i ,{\bf \overline b}^a_i)+\frac{\partial \Delta \overline {\bf v}_{ij} }{\partial{\bf \overline b}^g} \delta{\bf b}^g_i+\frac{\partial \Delta \overline {\bf v}_{ij} }{\partial \overline {\bf b}^a} \delta{\bf b}^a_i \\ \Delta \tilde {\bf p}_{ij} ({\bf \hat b}^g_i ,{\bf \hat b}^a_i) \approx \Delta \tilde {\bf v}_{ij} ({\bf \overline b}^g_i ,{\bf \overline b}^a_i)+\frac{\partial \Delta \overline {\bf p}_{ij} }{\partial{\bf \overline b}^g} \delta{\bf b}^g_i+\frac{\partial \Delta \overline {\bf p}_{ij} }{\partial \overline {\bf b}^a} \delta{\bf b}^a_i \tag{50}$ΔR~ij​(b^ig​)≈ΔR~ij​(big​)⋅Exp(∂bg∂ΔRij​​δbig​)Δv~ij​(b^ig​,b^ia​)≈Δv~ij​(big​,bia​)+∂bg∂Δvij​​δbig​+∂ba∂Δvij​​δbia​Δp~​ij​(b^ig​,b^ia​)≈Δv~ij​(big​,bia​)+∂bg∂Δp​ij​​δbig​+∂ba∂Δp​ij​​δbia​(50)
爲了求得雅克比係數，進行如下定義：
$\Delta \hat {\bf R}_{ij}=\Delta \tilde {\bf R}_{ij} ({\bf \hat b}^g_i ) , \ \Delta \overline {\bf R}_{ij}=\Delta \tilde {\bf R}_{ij} ({\bf \overline b}^g_i ) \\ \Delta \hat {\bf v}_{ij}=\Delta \tilde {\bf v}_{ij} ({\bf \hat b}^g_i ) , \ \Delta \overline {\bf v}_{ij}=\Delta \tilde {\bf v}_{ij} ({\bf \overline b}^g_i ) \\ \Delta \hat {\bf p}_{ij}=\Delta \tilde {\bf p}_{ij} ({\bf \hat b}^g_i ) , \ \Delta \overline {\bf p}_{ij}=\Delta \tilde {\bf p}_{ij} ({\bf \overline b}^g_i ) \tag{51}$ΔR^ij​=ΔR~ij​(b^ig​), ΔRij​=ΔR~ij​(big​)Δv^ij​=Δv~ij​(b^ig​), Δvij​=Δv~ij​(big​)Δp^​ij​=Δp~​ij​(b^ig​), Δp​ij​=Δp~​ij​(big​)(51)
使用與式(32)和(43)相同的方法，推導得到
$\begin{aligned} \Delta \hat {\bf R}_{ij}&=\Delta \tilde {\bf R}_{ij} ({\bf \hat b}^g_i ) \\ &=\prod_{k=i}^{j-1}\ {\rm Exp}((\tilde \omega_k-b_i^g)\Delta t) \\ &=\prod_{k=i}^{j-1}\ {\rm Exp}((\tilde \omega_k-\overline b_i^g - \delta b_i^g)\Delta t ) \\ &=\prod_{k=i}^{j-1}\ {\rm Exp}((\tilde \omega_k- \overline b_i^g)\Delta t) {\rm Exp}(-{\rm J}_r^k \delta{\bf b}^g_i\Delta t) \\ &=\Delta \overline {\bf R}_{ij} \prod_{k=i}^{j-1}{\rm Exp}(-\Delta \tilde {\bf R}_{k+1j}(\overline{\bf b}_i)^{\rm T} {\rm J}_r^k \delta{\bf b}^g_i\Delta t) \\ &=\Delta \overline {\bf R}_{ij}{\rm Exp}( \sum_{k=i}^{j-1} -\Delta \tilde {\bf R}_{k+1j}(\overline{\bf b}_i)^{\rm T} {\rm J}_r^k \delta{\bf b}^g_i\Delta t) \end{aligned} \tag{52}$ΔR^ij​​=ΔR~ij​(b^ig​)=k=i∏j−1​ Exp((ω~k​−big​)Δt)=k=i∏j−1​ Exp((ω~k​−big​−δbig​)Δt)=k=i∏j−1​ Exp((ω~k​−big​)Δt)Exp(−Jrk​δbig​Δt)=ΔRij​k=i∏j−1​Exp(−ΔR~k+1j​(bi​)TJrk​δbig​Δt)=ΔRij​Exp(k=i∑j−1​−ΔR~k+1j​(bi​)TJrk​δbig​Δt)​(52)
將其代入 $\Delta \hat {\bf p}_{ij}, \ \Delta \hat {\bf v}_{ij}$Δp^​ij​, Δv^ij​之中，並對Exp進行一階近似可推導出其它雅克比係數。
$\begin{aligned} \frac{\partial \Delta \overline {\bf R}_{ij} }{\partial{\bf \overline b}^g} &=-\sum_{k=i}^{j-1}[ \Delta \tilde {\bf R}_{k+1j}(\overline{\bf b}_i)^{\rm T} {\rm J}_r^k \Delta t] \\ \frac{\partial \Delta \overline {\bf v}_{ij} }{\partial{\bf \overline b}^a} &= -\sum_{k=i}^{j-1}\Delta \overline {\bf R}_{ik} \Delta t \\ \frac{\partial \Delta \overline {\bf v}_{ij} }{\partial{\bf \overline b}^g} &= -\sum_{k=i}^{j-1}\Delta \overline {\bf R}_{ik}(\overline {\bf a}_k-\overline{\bf b}_i^a)^\wedge \frac{\partial \Delta \overline {\bf R}_{ik} }{\partial \overline {\bf b}^g}\Delta t \\ \frac{\partial \Delta \overline {\bf p}_{ij} }{\partial{\bf \overline b}^a} &= \sum_{k=i}^{j-1} \frac{\partial \Delta \overline {\bf v}_{ik} }{\partial \overline {\bf b}^a}\Delta t- \frac{1}{2}\Delta \overline {\bf R}_{ik} \Delta t^2 \\ \frac{\partial \Delta \overline {\bf p}_{ij} }{\partial{\bf \overline b}^g} &= \sum_{k=i}^{j-1} \frac{\partial \Delta \overline {\bf p}_{ik} }{\partial \overline {\bf b}^g}\Delta t- \frac{1}{2}\Delta \overline {\bf R}_{ik}(\overline {\bf a}_k-\overline{\bf b}_i^a)^\wedge \frac{\partial \Delta \overline {\bf R}_{ik} }{\partial \overline {\bf b}^g}\Delta t^2 \end{aligned}$∂bg∂ΔRij​​∂ba∂Δvij​​∂bg∂Δvij​​∂ba∂Δp​ij​​∂bg∂Δp​ij​​​=−k=i∑j−1​[ΔR~k+1j​(bi​)TJrk​Δt]=−k=i∑j−1​ΔRik​Δt=−k=i∑j−1​ΔRik​(ak​−bia​)∧∂bg∂ΔRik​​Δt=k=i∑j−1​∂ba∂Δvik​​Δt−21​ΔRik​Δt2=k=i∑j−1​∂bg∂Δp​ik​​Δt−21​ΔRik​(ak​−bia​)∧∂bg∂ΔRik​​Δt2​

D. Preintegrated IMU Factors

預積分測量模型使用了大量的一階近似和高斯分佈近似，因此存在殘差。殘差是預積分的計算值（使用非IMU方法，如視覺）與測量值之間的差。優化最終的目的就是通過調整（lifting)導航狀態使殘差（的加權範數）最小化。

根據公式（39）可以得到殘差的定義：
${\bf r}_{\mathcal I_{ij}} \doteq [{\bf r}^{\rm T}_{\Delta {\bf R}_{ij}},\ {\bf r}^{\rm T}_{\Delta {\bf v}_{ij}}, \ {\bf r}^{\rm T}_{\Delta {\bf p}_{ij}}]^{\rm T} \in \R^9 \tag{53}$rIij​​≐[rΔRij​T​, rΔvij​T​, rΔpij​T​]T∈R9(53)

$\begin{aligned} {\bf r}_{\Delta {\bf R}_{ij}} &\doteq {\rm Log}\left( \left(\Delta \tilde {\bf R}_{ij} (\overline {\bf b}^g_i) \cdot {\rm Exp} \left( \frac{\partial \Delta \overline {\bf R}_{ij} }{\partial{\bf \overline b}^g} \delta{\bf b}^g_i \right) \right)^{\rm T} {\bf R}_i^{\rm T}{\bf R}_j \right) \\ &= {\rm Log} [(\Delta \hat {\bf R}_{ij})^T\Delta {\bf R}_{ij}]\\ \\ {\bf r}_{\Delta {\bf v}_{ij}} &\doteq {\bf R}_i^T({\bf v}_j-{\bf v}_i-{\bf g}\Delta t_{ij}) \\ &- \left[\Delta \tilde {\bf v}_{ij} ({\bf \overline b}^g_i ,{\bf \overline b}^a_i)+\frac{\partial \Delta \overline {\bf v}_{ij} }{\partial{\bf \overline b}^g} \delta{\bf b}^g_i+\frac{\partial \Delta \overline {\bf v}_{ij} }{\partial \overline {\bf b}^a} \delta{\bf b}^a_i \right]\\ &= \Delta {\bf v}_{ij} -\Delta \hat {\bf v}_{ij}\\ \\ {\bf r}_{\Delta {\bf p}_{ij}} &\doteq {\bf R}_i^T({\bf p}_j-{\bf p}_i-{\bf v}_i \Delta t_{ij}-\frac{1}{2}{\bf g}\Delta t_{ij}^2) \\ &- \left[\Delta \tilde {\bf p}_{ij} ({\bf \overline b}^g_i ,{\bf \overline b}^a_i)+\frac{\partial \Delta \overline {\bf p}_{ij} }{\partial{\bf \overline b}^g} \delta{\bf b}^g_i+\frac{\partial \Delta \overline {\bf p}_{ij} }{\partial \overline {\bf b}^a} \delta{\bf b}^a_i \right] \\ &=\Delta {\bf p}_{ij} - \Delta \hat {\bf p}_{ij} \end{aligned} \tag{54}$rΔRij​​rΔvij​​rΔpij​​​≐Log((ΔR~ij​(big​)⋅Exp(∂bg∂ΔRij​​δbig​))TRiT​Rj​)=Log[(ΔR^ij​)TΔRij​]≐RiT​(vj​−vi​−gΔtij​)−[Δv~ij​(big​,bia​)+∂bg∂Δvij​​δbig​+∂ba∂Δvij​​δbia​]=Δvij​−Δv^ij​≐RiT​(pj​−pi​−vi​Δtij​−21​gΔtij2​)−[Δp~​ij​(big​,bia​)+∂bg∂Δp​ij​​δbig​+∂ba∂Δp​ij​​δbia​]=Δpij​−Δp^​ij​​(54)

bias常常被當做狀態量進行估計，使用估計其偏差的方式，因此全部的導航狀態爲 ${\bf R}_i,{\bf p}_i,{\bf v}_i,{\bf R}_j,{\bf p}_j,{\bf v}_j,{\bf \delta b}_i^g,{\bf \delta b}_i^a$Ri​,pi​,vi​,Rj​,pj​,vj​,δbig​,δbia​。

注: lifting我理解的是，在待優化變量上增加一個微小量進行更新，通過調整這個微小量達到優化的目的。

D1. Jacobians of Residual Errors

(1). ${\bf r}_{\Delta {\bf R}_{ij}}$rΔRij​​的雅克比

根據公式（54）的第一個式子可知：

${\bf p}_i, \ {\bf p}_j, \ {\bf v}_i, \ {\bf v}_j, \ \delta{\bf b}_i^a$pi​, pj​, vi​, vj​, δbia​未出現在公式中，因此雅克比爲0；

${\bf R}_i$Ri​進行lifting計算雅克比：
$\begin{aligned} &{\bf r}_{\Delta{\bf R}_{ij}}({\bf R}_i{\rm Exp}(\delta\phi_i)) \\ &={\rm Log}\left[ (\Delta\hat{\bf R}_{ij})^T({\bf R}_i{\rm Exp}(\delta\phi_i))^T{\bf R}_j \right] {\text （展開轉置並將裝置用負號代替）} \\ &={\rm Log}\left[ (\Delta\hat{\bf R}_{ij})^T{\rm Exp}(-\delta\phi_i){\bf R}_i^T{\bf R}_j \right] {\text （ 使用Adjoint性質} ）\\ &={\rm Log}\left[ (\Delta\hat{\bf R}_{ij})^T{\bf R}_i^T{\bf R}_j{\rm Exp}(-{\bf R}_j^T{\bf R}_i\delta\phi_i) \right] {\text （前邊是原殘差公式）} \\ &={\rm Log}\left [ {\rm Exp}\left({\bf r}_{\Delta{\bf R}_{ij}}({\bf R}_i) \right)\cdot {\rm Exp}(-{\bf R}_j^T{\bf R}_i\delta\phi_i) \right ] {\text（使用BCH近似公式）} \\ &\approx{\bf r}_{\Delta{\bf R}_{ij}}({\bf R}_i)-{\bf J}_r^{-1}\left({\bf r}_{\Delta{\bf R}_{ij}}({\bf R}_i)\right){\bf R}_j^T{\bf R}_i\delta\phi_i \\ &={\bf r}_{\Delta{\bf R}_{ij}}-{\bf J}_r^{-1}\left({\bf r}_{\Delta{\bf R}_{ij}}\right){\bf R}_j^T{\bf R}_i\delta\phi_i \end{aligned}$​rΔRij​​(Ri​Exp(δϕi​))=Log[(ΔR^ij​)T(Ri​Exp(δϕi​))TRj​]（展開轉置並將裝置用負號代替）=Log[(ΔR^ij​)TExp(−δϕi​)RiT​Rj​]（使用Adjoint性質）=Log[(ΔR^ij​)TRiT​Rj​Exp(−RjT​Ri​δϕi​)]（前邊是原殘差公式）=Log[Exp(rΔRij​​(Ri​))⋅Exp(−RjT​Ri​δϕi​)]（使用BCH近似公式）≈rΔRij​​(Ri​)−Jr−1​(rΔRij​​(Ri​))RjT​Ri​δϕi​=rΔRij​​−Jr−1​(rΔRij​​)RjT​Ri​δϕi​​
${\bf R}_j$Rj​：
$\begin{aligned} &{\bf r}_{\Delta{\bf R}_{ij}}({\bf R}_i{\rm Exp}(\delta\phi_i)) \\ &={\rm Log}\left[ (\Delta\hat{\bf R}_{ij})^T{\bf R}_i^T{\bf R}_j{\rm Exp}(\delta\phi_i) \right] {\text （前邊原公式，並利用BCH公式）} \\ &={\bf r}_{\Delta{\bf R}_{ij}}({\bf R}_j)+{\bf J}_r^{-1}({\bf r}_{\Delta{\bf R}_{ij}}({\bf R}_j)) \delta \phi_j \\ &={\bf r}_{\Delta{\bf R}_{ij}}+{\bf J}_r^{-1}({\bf r}_{\Delta{\bf R}_{ij}}) \delta \phi_j \end{aligned}$​rΔRij​​(Ri​Exp(δϕi​))=Log[(ΔR^ij​)TRiT​Rj​Exp(δϕi​)]（前邊原公式，並利用BCH公式）=rΔRij​​(Rj​)+Jr−1​(rΔRij​​(Rj​))δϕj​=rΔRij​​+Jr−1​(rΔRij​​)δϕj​​
$\delta{\bf b}_i^g $：
$\begin{aligned} &{\bf r}_{\Delta{\bf R}_{ij}}(\delta{\bf b}_i^g+\tilde\delta{\bf b}_i^g) \\ &={\rm Log}\left\{ \left [ \Delta \tilde {\bf R}_{ij} (\overline {\bf b}^g_i) \cdot {\rm Exp} \left( \frac{\partial \Delta \overline {\bf R}_{ij} }{\partial{\bf \overline b}^g} (\delta{\bf b}^g_i + \tilde \delta{\bf b}_i^g\right) \right]^T {\bf R}_i^T{\bf R}_j\right\} \\ &{\text (將Exp使用BCH近似公式展開) }\\ &\approx {\rm Log}\left\{ \left [ \Delta \tilde {\bf R}_{ij} (\overline {\bf b}^g_i) \cdot {\rm Exp} \left( \frac{\partial \Delta \overline {\bf R}_{ij} }{\partial{\bf \overline b}^g} \delta{\bf b}^g_i \right) {\rm Exp} \left({\bf J}_r \left( \frac{ \partial \Delta \overline {\bf R}_{ij} }{\partial{\bf \overline b}^g} \delta{\bf b}^g_i\right)\frac{ \partial \Delta \overline {\bf R}_{ij} }{\partial{\bf \overline b}^g} \tilde \delta{\bf b}_i^g \right) \right]^T \Delta{\bf R}_{ij}\right\} \\ &{\text （第二個Exp之前是預積分增量） }\\ &= {\rm Log}\left\{ \left [ \Delta \hat {\bf R}_{ij} \cdot {\rm Exp} \left({\bf J}_r \frac{ \partial \Delta \overline {\bf R}_{ij} }{\partial{\bf \overline b}^g} \tilde \delta{\bf b}_i^g \right) \right]^T \Delta{\bf R}_{ij}\right\} \\ &{\text （展開轉置部分，合併第二部分） }\\ &= {\rm Log}\left[ {\rm Exp} \left( -{\bf J}_r \frac{ \partial \Delta \overline {\bf R}_{ij} }{\partial{\bf \overline b}^g} \tilde \delta{\bf b}_i^g \right) {\rm Exp}({\bf r}_{\Delta{\bf R}_{ij}}(\delta{\bf b}_i^g)) \right] \\ &{\text （使用Adjoint性質，把微小量換到右側爲了J_r） } \\ &= {\rm Log}\left[ {\rm Exp}({\bf r}_{\Delta{\bf R}_{ij}}(\delta{\bf b}_i^g)) {\rm Exp} \left( - {\rm Exp}(-{\bf r}_{\Delta{\bf R}_{ij}}(\delta{\bf b}_i^g)){\bf J}_r \frac{ \partial \Delta \overline {\bf R}_{ij} }{\partial{\bf \overline b}^g} \tilde \delta{\bf b}_i^g \right) \right] \\ &{\text （使用BCH近似公式，這裏有兩個J_r注意區分） } \\ &={\bf r}_{\Delta{\bf R}_{ij}}-{\bf J}_r^{-1}({\bf r}_{\Delta{\bf R}_{ij}})\cdot{\rm Exp}(-{\bf r}_{\Delta{\bf R}_{ij}})\cdot {\bf J}_r \left( \frac{ \partial \Delta \overline {\bf R}_{ij} }{\partial{\bf \overline b}^g} \delta{\bf b}^g_i\right) \frac{ \partial \Delta \overline {\bf R}_{ij} }{\partial{\bf \overline b}^g} \tilde \delta{\bf b}_i^g \end{aligned}$​rΔRij​​(δbig​+δ~big​)=Log{[ΔR~ij​(big​)⋅Exp(∂bg∂ΔRij​​(δbig​+δ~big​)]TRiT​Rj​}(將Exp使用BCH近似公式展開)≈Log{[ΔR~ij​(big​)⋅Exp(∂bg∂ΔRij​​δbig​)Exp(Jr​(∂bg∂ΔRij​​δbig​)∂bg∂ΔRij​​δ~big​)]TΔRij​}（第二個Exp之前是預積分增量）=Log{[ΔR^ij​⋅Exp(Jr​∂bg∂ΔRij​​δ~big​)]TΔRij​}（展開轉置部分，合並第二部分）=Log[Exp(−Jr​∂bg∂ΔRij​​δ~big​)Exp(rΔRij​​(δbig​))]（使用Adjoint性質，把微小量換到右側爲了Jr​）=Log[Exp(rΔRij​​(δbig​))Exp(−Exp(−rΔRij​​(δbig​))Jr​∂bg∂ΔRij​​δ~big​)]（使用BCH近似公式，這裏有兩個Jr​注意區分）=rΔRij​​−Jr−1​(rΔRij​​)⋅Exp(−rΔRij​​)⋅Jr​(∂bg∂ΔRij​​δbig​)∂bg∂ΔRij​​δ~big​​
總結：
$\begin{aligned} \frac{\partial {\bf r}_{\Delta{\bf R}_{ij}}}{\partial\delta\phi_i}&= -{\bf J}_r^{-1}\left({\bf r}_{\Delta{\bf R}_{ij}}\right){\bf R}_j^T{\bf R}_i & \frac{\partial {\bf r}_{\Delta{\bf R}_{ij}}}{\partial\delta\phi_j} &={\bf J}_r^{-1}({\bf r}_{\Delta{\bf R}_{ij}}) \\ \frac{\partial {\bf r}_{\Delta{\bf R}_{ij}}}{\partial\delta {\bf p}_i}&=0 & \frac{\partial {\bf r}_{\Delta{\bf R}_{ij}}}{\partial\delta {\bf p}_j} &=0 \\ \frac{\partial {\bf r}_{\Delta{\bf R}_{ij}}}{\partial\delta {\bf v}_i}&=0 &\frac{\partial {\bf r}_{\Delta{\bf R}_{ij}}}{\partial\delta {\bf v}_j}& = 0\\ \frac{\partial {\bf r}_{\Delta{\bf R}_{ij}}}{\partial \tilde\delta{\bf b}^a_i}&= 0 &\frac{\partial {\bf r}_{\Delta{\bf R}_{ij}}}{\partial\tilde\delta{\bf b}^g_i} &= \alpha \\ \end{aligned}$∂δϕi​∂rΔRij​​​∂δpi​∂rΔRij​​​∂δvi​∂rΔRij​​​∂δ~bia​∂rΔRij​​​​=−Jr−1​(rΔRij​​)RjT​Ri​=0=0=0​∂δϕj​∂rΔRij​​​∂δpj​∂rΔRij​​​∂δvj​∂rΔRij​​​∂δ~big​∂rΔRij​​​​=Jr−1​(rΔRij​​)=0=0=α​
其中 $\alpha ={\bf J}_r^{-1}({\bf r}_{\Delta{\bf R}_{ij}})\cdot{\rm Exp}(-{\bf r}_{\Delta{\bf R}_{ij}})\cdot {\bf J}_r \left( \frac{ \partial \Delta \overline {\bf R}_{ij} }{\partial{\bf \overline b}^g} \delta{\bf b}^g_i\right) \frac{ \partial \Delta \overline {\bf R}_{ij} }{\partial{\bf \overline b}^g}$α=Jr−1​(rΔRij​​)⋅Exp(−rΔRij​​)⋅Jr​(∂bg∂ΔRij​​δbig​)∂bg∂ΔRij​​;

(2). ${\bf r}_{\Delta {\bf v}_{ij}}$rΔvij​​的雅克比

根據公式（54）的第二個式子可知：

${\bf R}_j, \ {\bf p}_i , \ {\bf p}_j $未出現在公式中雅克比爲0；

$\delta{\bf b}_i^a, \ \delta{\bf b}_i^g$δbia​, δbig​在公式中是線性的因此雅克比即爲係數；

${\bf R}_i$Ri​：
$\begin{aligned} &{\bf r}_{\Delta{\bf v}_{ij}}({\bf R}_i{\rm Exp}(\delta\phi_i)) \\ &=({\bf R}_i{\rm Exp}(\delta\phi_i))^T({\bf v}_j-{\bf v}_i-{\bf g}\Delta t_{ij})-\Delta\hat{ \bf v}_{ij} \\ &{\text （展開轉置，對Exp進行一階近似）}\\ &\approx\left({\bf I} -(\delta\phi_i)^\wedge \right)\cdot{\bf R}^T_i \cdot({\bf v}_j-{\bf v}_i-{\bf g}\Delta t_{ij})-\Delta\hat{ \bf v}_{ij} \\ &{\text （展開第一個括號，湊成一項原殘差量）} \\ &={\bf r}_{\Delta{\bf v}_{ij}} - (\delta\phi_i)^\wedge\cdot{\bf R}^T_i \cdot({\bf v}_j-{\bf v}_i-{\bf g}\Delta t_{ij}) \\ &{\text （利用a^\wedge b=-b^\wedge a推導）} \\ &={\bf r}_{\Delta{\bf v}_{ij}} + \left[ {\bf R}^T_i \cdot({\bf v}_j-{\bf v}_i-{\bf g}\Delta t_{ij}) \right]^\wedge\delta\phi_i \end{aligned}$​rΔvij​​(Ri​Exp(δϕi​))=(Ri​Exp(δϕi​))T(vj​−vi​−gΔtij​)−Δv^ij​（展開轉置，對Exp進行一階近似）≈(I−(δϕi​)∧)⋅RiT​⋅(vj​−vi​−gΔtij​)−Δv^ij​（展開第一個括號，湊成一項原殘差量）=rΔvij​​−(δϕi​)∧⋅RiT​⋅(vj​−vi​−gΔtij​)（利用a∧b=−b∧a推導）=rΔvij​​+[RiT​⋅(vj​−vi​−gΔtij​)]∧δϕi​​
${\bf v}_i$vi​：
$\begin{aligned} &{\bf r}_{\Delta{\bf v}_{ij}}({\bf v}_i+\delta{\bf v}_i) \\ &={\bf R}_i^T({\bf v}_j-{\bf v}_i-\delta{\bf v}_i-{\bf g}\Delta t_{ij})-\Delta\hat{ \bf v}_{ij} \\ &={\bf r}_{\Delta{\bf v}_{ij}}({\bf v}_i) -{\bf R}_i^T\delta{\bf v}_i \end{aligned}$​rΔvij​​(vi​+δvi​)=RiT​(vj​−vi​−δvi​−gΔtij​)−Δv^ij​=rΔvij​​(vi​)−RiT​δvi​​
${\bf v}_j$vj​：
$\begin{aligned} &{\bf r}_{\Delta{\bf v}_{ij}}({\bf v}_j+\delta{\bf v}_j) \\ &={\bf R}_i^T({\bf v}_j+\delta{\bf v}_j-{\bf v}_i-{\bf g}\Delta t_{ij})-\Delta\hat{ \bf v}_{ij} \\ &={\bf r}_{\Delta{\bf v}_{ij}}({\bf v}_j) +{\bf R}_i^T\delta{\bf v}_j \end{aligned}$​rΔvij​​(vj​+δvj​)=RiT​(vj​+δvj​−vi​−gΔtij​)−Δv^ij​=rΔvij​​(vj​)+RiT​δvj​​
總結：
$\begin{aligned} \frac{\partial {\bf r}_{\Delta{\bf v}_{ij}}}{\partial\delta\phi_i}&= \left[ {\bf R}^T_i \cdot({\bf v}_j-{\bf v}_i-{\bf g}\Delta t_{ij}) \right]^\wedge & \frac{\partial {\bf r}_{\Delta{\bf v}_{ij}}}{\partial\delta\phi_j} &= 0\\ \frac{\partial {\bf r}_{\Delta{\bf v}_{ij}}}{\partial\delta {\bf p}_i}&=0 & \frac{\partial {\bf r}_{\Delta{\bf v}_{ij}}}{\partial\delta {\bf p}_j} &=0 \\ \frac{\partial {\bf r}_{\Delta{\bf v}_{ij}}}{\partial\delta {\bf v}_i}&=-{\bf R}^T &\frac{\partial {\bf r}_{\Delta{\bf v}_{ij}}}{\partial\delta {\bf v}_j}& = {\bf R}^T \\ \frac{\partial {\bf r}_{\Delta{\bf v}_{ij}}}{\partial \tilde\delta{\bf b}^a_i}&= - \frac{\partial \Delta \overline {\bf v}_{ij} }{\partial{\bf \overline b}^a_i} &\frac{\partial {\bf r}_{\Delta{\bf v}_{ij}}}{\partial\tilde\delta{\bf b}^g_i} &= -\frac{\partial \Delta \overline {\bf v}_{ij} }{\partial{\bf \overline b}^g_i} \\ \end{aligned}$∂δϕi​∂rΔvij​​​∂δpi​∂rΔvij​​​∂δvi​∂rΔvij​​​∂δ~bia​∂rΔvij​​​​=[RiT​⋅(vj​−vi​−gΔtij​)]∧=0=−RT=−∂bia​∂Δvij​​​∂δϕj​∂rΔvij​​​∂δpj​∂rΔvij​​​∂δvj​∂rΔvij​​​∂δ~big​∂rΔvij​​​​=0=0=RT=−∂big​∂Δvij​​​
(3). ${\bf r}_{\Delta {\bf p}_{ij}}$rΔpij​​的雅克比

根據公式（54）的第三個式子可知：

${\bf R}_j , \ {\bf v}_j $未出現在公式中雅克比爲0；

$\delta{\bf b}_i^a, \ \delta{\bf b}_i^g$δbia​, δbig​在公式中是線性的因此雅克比即爲係數；

${\bf R}_i$Ri​：
$\begin{aligned} &{\bf r}_{\Delta{\bf p}_{ij}}({\bf R}_i{\rm Exp}(\delta\phi_i)) \\ &=({\bf R}_i{\rm Exp}(\delta\phi_i)) ^T\left({\bf p}_j-{\bf p}_i-{\bf v}_i \Delta t_{ij}-\frac{1}{2}{\bf g}\Delta t_{ij}^2 \right)-\Delta\hat{\bf p}_{ij} \\ &{\text （展開轉置項，對Exp進行一階近似）} \\ &\approx \left( {\bf I}-(\delta\phi_i)^\wedge \right){\bf R}_i^T\left({\bf p}_j-{\bf p}_i-{\bf v}_i \Delta t_{ij}-\frac{1}{2}{\bf g}\Delta t_{ij}^2 \right)-\Delta\hat{\bf p}_{ij} \\ &{\text （乘開，並且跟之前一樣）} \\ &={\bf r}_{\Delta{\bf p}_{ij}}+\left[ {\bf R}_i^T\left({\bf p}_j-{\bf p}_i-{\bf v}_i \Delta t_{ij}-\frac{1}{2}{\bf g}\Delta t_{ij}^2 \right) \right]^\wedge \cdot\delta\phi_i \end{aligned}$​rΔpij​​(Ri​Exp(δϕi​))=(Ri​Exp(δϕi​))T(pj​−pi​−vi​Δtij​−21​gΔtij2​)−Δp^​ij​（展開轉置項，對Exp進行一階近似）≈(I−(δϕi​)∧)RiT​(pj​−pi​−vi​Δtij​−21​gΔtij2​)−Δp^​ij​（乘開，並且跟之前一樣）=rΔpij​​+[RiT​(pj​−pi​−vi​Δtij​−21​gΔtij2​)]∧⋅δϕi​​
${\bf p}_i$pi​：
$\begin{aligned} &{\bf r}_{\Delta{\bf p}_{ij}}({\bf p}_i+{\bf R}_i \cdot\delta{\bf p}_i) \\ &={\bf R}_i ^T\left({\bf p}_j-{\bf p}_i-{\bf R}_i \cdot\delta{\bf p}_i-{\bf v}_i \Delta t_{ij}-\frac{1}{2}{\bf g}\Delta t_{ij}^2 \right)-\Delta\hat{\bf p}_{ij} \\ &={\bf R}_i ^T\left({\bf p}_j-{\bf p}_i-{\bf v}_i \Delta t_{ij}-\frac{1}{2}{\bf g}\Delta t_{ij}^2 \right)-\Delta\hat{\bf p}_{ij} -{\bf I}\cdot\delta{\bf p}_i\\ &={\bf r}_{\Delta{\bf p}_{ij}}-{\bf I}\cdot\delta{\bf p}_i \end{aligned}$​rΔpij​​(pi​+Ri​⋅δpi​)=RiT​(pj​−pi​−Ri​⋅δpi​−vi​Δtij​−21​gΔtij2​)−Δp^​ij​=RiT​(pj​−pi​−vi​Δtij​−21​gΔtij2​)−Δp^​ij​−I⋅δpi​=rΔpij​​−I⋅δpi​​
${\bf p}_j$pj​：
$\begin{aligned} &{\bf r}_{\Delta{\bf p}_{ij}}({\bf p}_j+{\bf R}_j \cdot\delta{\bf p}_j) \\ &={\bf R}_i ^T\left({\bf p}_j+{\bf R}_j \cdot\delta{\bf p}_j -{\bf p}_i-{\bf v}_i \Delta t_{ij}-\frac{1}{2}{\bf g}\Delta t_{ij}^2 \right)-\Delta\hat{\bf p}_{ij} \\ &={\bf R}_i ^T\left({\bf p}_j-{\bf p}_i-{\bf v}_i \Delta t_{ij}-\frac{1}{2}{\bf g}\Delta t_{ij}^2 \right)-\Delta\hat{\bf p}_{ij} +{\bf R}_i ^T{\bf R}_j \cdot\delta{\bf p}_j\\ &={\bf r}_{\Delta{\bf p}_{ij}}+{\bf R}_i ^T{\bf R}_j \cdot\delta{\bf p}_j \end{aligned}$​rΔpij​​(pj​+Rj​⋅δpj​)=RiT​(pj​+Rj​⋅δpj​−pi​−vi​Δtij​−21​gΔtij2​)−Δp^​ij​=RiT​(pj​−pi​−vi​Δtij​−21​gΔtij2​)−Δp^​ij​+RiT​Rj​⋅δpj​=rΔpij​​+RiT​Rj​⋅δpj​​
${\bf v}_i$vi​：
$\begin{aligned} &{\bf r}_{\Delta{\bf p}_{ij}}({\bf v}_i+\delta{\bf v}_i) \\ &={\bf R}_i ^T\left({\bf p}_j -{\bf p}_i-{\bf v}_i \Delta t_{ij}-\delta{\bf v}_i \Delta t_{ij}-\frac{1}{2}{\bf g}\Delta t_{ij}^2 \right)-\Delta\hat{\bf p}_{ij} \\ &={\bf r}_{\Delta{\bf p}_{ij}}-{\bf R}_i ^T\Delta t_{ij}\cdot\delta{\bf v}_i \end{aligned}$​rΔpij​​(vi​+δvi​)=RiT​(pj​−pi​−vi​Δtij​−δvi​Δtij​−21​gΔtij2​)−Δp^​ij​=rΔpij​​−RiT​Δtij​⋅δvi​​
總結：
$\begin{aligned} \frac{\partial {\bf r}_{\Delta{\bf p}_{ij}}}{\partial\delta\phi_i}&= \left[ {\bf R}_i^T\left({\bf p}_j-{\bf p}_i-{\bf v}_i \Delta t_{ij}-\frac{1}{2}{\bf g}\Delta t_{ij}^2 \right) \right]^\wedge & \frac{\partial {\bf r}_{\Delta{\bf p}_{ij}}}{\partial\delta\phi_j} &= 0\\ \frac{\partial {\bf r}_{\Delta{\bf p}_{ij}}}{\partial\delta {\bf p}_i}&=-{\bf I}_{3\times1} & \frac{\partial {\bf r}_{\Delta{\bf p}_{ij}}}{\partial\delta {\bf p}_j} &={\bf R}_i ^T{\bf R}_j \\ \frac{\partial {\bf r}_{\Delta{\bf p}_{ij}}}{\partial\delta {\bf v}_i}&=-{\bf R}_i ^T\Delta t_{ij} &\frac{\partial {\bf r}_{\Delta{\bf p}_{ij}}}{\partial\delta {\bf v}_j}& =0 \\ \frac{\partial {\bf r}_{\Delta{\bf p}_{ij}}}{\partial \tilde\delta{\bf b}^a_i}&= - \frac{\partial \Delta \overline {\bf p}_{ij} }{\partial{\bf \overline b}^a_i} &\frac{\partial {\bf r}_{\Delta{\bf p}_{ij}}}{\partial\tilde\delta{\bf b}^g_i} &= -\frac{\partial \Delta \overline {\bf p}_{ij} }{\partial{\bf \overline b}^g_i} \\ \end{aligned}$∂δϕi​∂rΔpij​​​∂δpi​∂rΔpij​​​∂δvi​∂rΔpij​​​∂δ~bia​∂rΔpij​​​​=[RiT​(pj​−pi​−vi​Δtij​−21​gΔtij2​)]∧=−I3×1​=−RiT​Δtij​=−∂bia​∂Δp​ij​​​∂δϕj​∂rΔpij​​​∂δpj​∂rΔpij​​​∂δvj​∂rΔpij​​​∂δ~big​∂rΔpij​​​​=0=RiT​Rj​=0=−∂big​∂Δp​ij​​​

七、無結構化視覺因子

對於視覺測量引入無結構化模型(structureless model)，關鍵就是線性化的消除MAP優化中的路標變量。對於視覺測量的殘差有以下模型：
$\sum_{i \in{\mathcal K}_k} \sum_{l\in{\mathcal C_i}}\|{\bf r}_{{\mathcal C}_{il}}\|^2_{\Sigma_{\mathcal C}}=\sum_{l=1}^{L}\sum_{i\in {\mathcal X}(l)}\|{\bf r}_{{\mathcal C}_{il}}\|^2_{\Sigma_{\mathcal C}} \\ {\bf r}_{{\mathcal C}_{il}}={\bf z}_{il}-\pi({\bf R}_i,{\bf p}_i,\rho_l) \tag{55}$i∈Kk​∑​l∈Ci​∑​∥rCil​​∥ΣC​2​=l=1∑L​i∈X(l)∑​∥rCil​​∥ΣC​2​rCil​​=zil​−π(Ri​,pi​,ρl​)(55)
公式的含義是對於L個路標中的一個，在其可被觀測到的關鍵幀上計算測量值與重投影之間的誤差的二範數。

其中 $\rho _l$ρl​是路標的位置，這會對計算產生負影響，因此structureless模型就是要避免優化路標。對代價函數進行縮放，原殘差公式變爲：
$\sum_{l=1}^{L}\sum_{i\in {\mathcal X}(l)}\| {\bf z}_{il}-\check \pi({\bf \delta \phi}_i,{\delta \bf p}_i,\delta\rho_l) \|^2_{\Sigma_{\mathcal C}} \tag{56}$l=1∑L​i∈X(l)∑​∥zil​−πˇ(δϕi​,δpi​,δρl​)∥ΣC​2​(56)
對其進行線性化, 並將第二個求和寫入矩陣中：
$\sum_{l=1}^{L}\| {\bf F}_l\delta{\bf T}_{\mathcal X(l)}+{\bf E}_l\delta \rho_l-{\bf b}_l \|^2 \tag{57}$l=1∑L​∥Fl​δT