寫在前面

最近看了ESKF之後，收穫蠻大的，特別是對於四元數的殘差的推導，感覺給出了一個很標準的推導方式，因此就趁熱打鐵，把VINS中的預積分部分殘差推導了一下，也算是對上了VINS中imu_factor.h中的程式碼部分。

殘差

設狀態變數為$x=[Pi，Pj，Vi，Vj，Qi，Qj，Bai，Baj，Bgi，Bgj]$
兩幀之間預積分為$m=[\alpha_{ij}, \beta_{ij}, \theta_{ij}]$

殘差對於狀態變數的jacobian

其實我們都知道，在整個優化問題中，jacobian的求解還是比較重要的，因為它直接構成了增量方程，我們下面主要看一下關於上面殘差關於狀態變數的jacobian

a. 對於i時刻的位姿Pi，Qi

• $\delta P_{ij}$對於Pi，Qi的導數很明顯為
  $\frac{d\delta P_{ij}}{dPi}=-R_w^i\frac{d}{}$
  δPijdQi=dδPijdRiw=(RiwExp(δθ))T(Pj−Pi+0.5gt2−Vit)−(Riw)T(Pj−Pi+0.5gt2−Vit)δθ=(Riwδθ×)T(Pj−Pi+0.5gt2−Vit)=((Riw)T(Pj−Pi+0.5gt2−Vit))×δθ \frac{d\delta P_{ij}}{dPi}=-R_w^i \\ \frac{d\delta P_{ij}}{dQi}=\frac{d\delta P_{ij}}{dR_i^w}=\frac{(R_i^wExp(\delta \theta))^T(P_j-P_i+0.5gt^2-V_it)-(R_i^w)^T(P_j-P_i+0.5gt^2-V_it)}{\delta \theta} = (R_i^w\delta \theta_×)^T(P_j-P_i+0.5gt^2-V_it)=((R_i^w)^T(P_j-P_i+0.5gt^2-V_it))_×\delta\theta \\
• $\delta V_{ij}$對於Pi，Qi的導數很明顯為
  $\frac{d\delta V_{ij}}{dPi}=-R_w^i \\ \frac{d\delta V_{ij}}{dQi}=\frac{d\delta V_{ij}}{dR_i^w}=\frac{(R_i^wExp(\delta \theta))^T(V_j-V_i+gt)-(R_i^w)^T(V_j-V_i+gt)}{\delta \theta} = (R_i^w\delta \theta_×)^T(V_j-V_i+gt)=((R_i^w)^T(V_j-V_i+gt)_×\delta\theta$
• $\delta r_{ij}$對於Pi的導數很明顯為0，但是對於Qi的導數，這裡我們要進行一下簡單的推導

設$q_{ijt}=(q_i\otimes{\begin{bmatrix}1\\0.5\delta \theta\end{bmatrix}})^{-1}\otimes q_j,\quad q_{ij}=q_{ij}$

相關推薦

VINS-Mono預積分公式推導

寫在前面 最近看了ESKF之後，收穫蠻大的，特別是對於四元數的殘差的推導，感覺給出了一個很標準的推導方式，因此就趁熱打鐵，把VINS中的預積分部分殘差推導了一下，也算是對上了VINS中imu_factor.h中的程式碼部分。 殘差 設狀態變數為x=[Pi，Pj

IMU 預積分推導

接下來 範圍 del spa jacob 處的 HR 每次 過大 給 StereoDSO 加 IMU，想直接用 OKVIS 的代碼，但是有點看不懂。知乎上鄭帆寫的文章《四元數矩陣與 so(3) 左右雅可比 》提到 OKVIS 的預積分是使用四元數，而預積分論文中使用 so(

VINS詳解之IMU預積分（一）

看VINS很久了，但一直處於似懂非懂的狀態，因此想借著寫部落格的機會結合論文對程式碼進行詳細的梳理。第一篇就從IMU預積分開始吧。 一 、首先是接收IMU的message 1 void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg

POJ 2140 Herd Sums 公式推導

cpp true typedef 因數 固定 ostream highlight ring sum 題意:給出n<=1e7 求有多少個連續數之和等於k x+x+1+....x+k=n (k+1)k/2+(k+1)x=n (k+1)k+(k+1)2x=2*n (k

vijos - P1543極值問題(斐波那契數列 + 公式推導 + python)

找到 span add gin python3 abi pri n) fill P1543極值問題 Accepted 標簽：[顯示標簽] 背景 小銘的數學之旅2。 描寫敘述 已知m、n為整數，且滿足下列兩個條件： ①

vijos - P1302連續自然數和 (公式推導 + python)

.net ble tags ucid stat down bsp z-index fontsize P1302連續自然數和 Accepted 標簽：[顯示標簽] 描寫敘述 對一個給定的自然數M，求出所有的連續的自然數段（連續個數大於1）

解釋一下核主成分分析(Kernel Principal Component Analysis, KPCA)的公式推導過程（轉載）

線性不可分 itl 專註 out center forest 測試 重要 原因 KPCA，中文名稱”核主成分分析“，是對PCA算法的非線性擴展，言外之意，PCA是線性的，其對於非線性數據往往顯得無能為力，例如，不同人之間的人臉圖像，肯定存在非線性關系，自己做的基於ORL數據

SVM公式推導筆記

svm width org sin .org 參考資料 zhang www http 參考資料： 對偶函數-http://blog.pluskid.org/?p=702 KTT和拉格朗日乘子-http://www.cnblogs.com/zhangchaoyan

反向傳播算法（過程及公式推導）

不能 簡化 會有 geo 之前 代碼 求和 不同 eof 一、反向傳播的由來 在我們開始DL的研究之前，需要把ANN—人工神經元網絡以及bp算法做一個簡單解釋。關於ANN的結構，我不再多說，網上有大量的學習資料，主要就是搞清一些名詞：輸入層/輸入神經元，輸出層/輸出神經元，

機器學習之支持向量機（一）：支持向量機的公式推導

根據 監督式 art 通用 利用 哪些 這就是 在線 方法 註：關於支持向量機系列文章是借鑒大神的神作，加以自己的理解寫成的；若對原作者有損請告知，我會及時處理。轉載請標明來源。 序： 我在支持向量機系列中主要講支持向量機的公式推導，第一部分講到推出拉格朗日對偶函數的對偶因

VINS-Mono代碼分析與總結(二) 系統初始化

exp 簡單 都是 最小值 cnblogs 特征向量 vision 還要 per VINS-Mono代碼註釋：https://github.com/gaochq/VINS-Mono/tree/comment 註釋不完整，可以一起交流。 參考文獻 1 VINS-Mono: A

VINS-Mono代碼分析與總結(最終版)

highlight 不一致 cat 前言 邊緣 會有 tag 相對 歸一化 VINS-Mono代碼分析總結 參考文獻 1 VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator,

VINS-mono詳細解讀

fusion 尋找 結果 field 錯誤 記錄 不用 tomat IE VINS-mono詳細解讀 極品巧克力 前言 Vins-mono是香港科技大學開源的一個VIO算法，https://github.com/HKUST-Aerial-Robotics/VINS-Mo

最速降線問題公式推導

關系 工作 第一個 int sub dash calc quad 證明 　　以前對物理特別感興趣的時候就專門研究過一段時間的變分法，記得當時閱讀了一本十分不錯的書籍，其作者名挺有趣的—老大中先生的《變分法基礎》（真的很不錯的一本講變分法的書，有興趣的同學可以去看

機器學習 LR中的參數叠代公式推導——極大似然和梯度下降

jpg blog 我們 應該 圖片 最大似然 gpo 機器學習實戰 pos 機器學習 LR中的參數叠代公式推導——極大似然和梯度下降 Logistic本質上是一個基於條件概率的判別模型(DiscriminativeModel)。

線性回歸 及 正則化 公式推導

tail 損失函數 csdn .net net nbsp art 公式推導 模型 基礎公式： ?BA/?A = BT ?ATB/?A = B ?ATBA/?A = 2BA 模型函數： hθ(x) = xθ 無正則化損失函數： J(θ) = 1/2(Xθ-Y)2

點到超平面距離公式 推導

idt ima .net || mage sub detail eight 技術 公式： d = |wx0 + b|/||w||2 推導： 參考文獻： https://blog.csdn.net/yutao03081/article/details/7

貝葉斯公式推導

樣本 條件樣式 區別 str 假設 分享 們的 圖片 繼續 基本概念 樣本空間：{試驗所有可能結果}-->一個試驗所有可能結果的集合，用 Ω 表示。所以P(Ω) = 1 事件：樣本空間的一個子集。用A、B、C表示。 條件概率 其實

【CodeForces - 215B】【Olympic Medal】（數學公式推導）

題目： The World Programming Olympics Medal is a metal disk, consisting of two parts: the first part is a ring with outer radius of r1 cm,

VINS-Mono翻譯

Abstract 由攝像機和低成本慣性測量單元(IMU)組成的單目視覺慣性系統(VINS)構成了用於度量六自由度狀態估計的最小感測器套件。然而，由於缺乏直接距離測量，在IMU處理、估計器初始化、外部標定和非線性優化等方面提出了重大挑戰。在本文中，我們提出了一種魯棒的、通用的單目視覺慣性狀態估計器V