首先根据最大后验估计（Maximum a posteriori estimation，MAP）构建非线性优化的目标函数。
初始化过程通过线性求解直接会给出一个状态的初值，而非线性优化的过程关键在于求解增量方程，并不断迭代到最优点，需要在初值以及后续的迭代点附近线性化（泰勒展开保留一阶后平方构建高斯牛顿梯度下降的增量方程）：
在初值x附近泰勒展开
$f(x+Delta x) = f(x) + JDelta x$
$costFunction = [f(x+Delta x)]^{2}$
最小化costFunction的过程其实就是迭代求解增量方程
$HDelta 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收敛或者限制固定迭代次数保证计算量的平衡）；