首先根據最大后驗估計（Maximum a posteriori estimation，MAP）構建非線性優化的目標函數。

初始化過程通過線性求解直接會給出一個狀態的初值，而非線性優化的過程關鍵在于求解增量方程，并不斷迭代到最優點，需要在初值以及后續的迭代點附近線性化（泰勒展開保留一階后平方構建高斯牛頓梯度下降的增量方程）：

在初值x附近泰勒展開

$f(x+\Delta x) = f(x) + J\Delta x$

$costFunction = [f(x+\Delta x)]^{2}$

最小化costFunction的過程其實就是迭代求解增量方程

$H\Delta x = b$

其中$H = J^{T}J$，$b = -J^{T}f$

因此，首先需要定義狀態的增量，主要因為旋轉矩陣沒有加法，無法迭代求解，因此在SO(3)流形的正切空間上定義四元數的加法，并表示成最小坐標的李代數（軸轉角）形式

因此對應的error-state costFunction定義為：

注意該式求解的是迭代需要使用的增量，而不是初始化過程可以直接線性求解出的初值。這是優化過程和初始化過程中costFunction的主要不同點。?

接下來還需要建立對應的觀測方程去計算residual及其Jacobian，并且推到誤差的傳播方程。和初始化過程中對比，增加的有：

• IMU中的旋轉矩陣；
• 誤差傳播考慮旋轉矩陣及其與translation之間的相關性；

對比初始化和非線性優化過程：

• 都是基于Sliding Window，但是初始化中每組Sliding Window只會計算一次，如果沒有收斂到可以完成初始化，則擴大窗口大小，或者直接滑動窗口；
• Nonlinear Optimization中Sliding Window則是在固定的Sliding Window中求解增量方程迭代優化到滿足一定條件（residual收斂或者限制固定迭代次數保證計算量的平衡）；

?

轉載于:https://www.cnblogs.com/shang-slam/p/7081923.html

總結

以上是生活随笔為你收集整理的VINS（五）非线性优化与在线标定调整的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。