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