VINS（五）非線性優化與在線標定調整

首先根據最大後驗估計（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收斂或者限制固定迭代次數保證計算量的平衡）；