VINS后端优化损失函数

状态向量

VINS后端优化的状态向量
$$\mathrm{X}=\left[\begin{array}{c}{x}_0,x_1,\cdots ,x_n,x_c^b,{\lambda }_0,{\lambda }_1,\cdots ,{\lambda }_m\end{array}\right]$$
$$\begin{array}{rl}{x}_k& =\left[\begin{array}{c}{p}_{b_k}^w,v_{b_k}^w,q_{b_k}^w,b_a,b_g\end{array}\right]\\ x_c^b& =\left[\begin{array}{c}{p}_c^b,q_c^b\end{array}\right]\end{array}$$

式中，$x_k,k\in [0,n]$表示滑动窗口内的$(n+1)$个关键帧；
$x_c^b$表示Camera到IMU的外参；
${\lambda }_k,k\in [0,m]$表示$(m+1)$个3D路标点的逆深度；

优化过程中的误差状态量为：
$$\delta \mathrm{X}=\left[\begin{array}{c}\delta x_0,\delta x_1,\cdots ,\delta x_n,\delta x_c^b,\delta {\lambda }_0,\delta {\lambda }_1,\cdots ,\delta {\lambda }_m\end{array}\right]$$
$$\begin{array}{rl}\delta x_k& =\left[\begin{array}{c}\delta p_{b_k}^w,\delta v_{b_k}^w,\delta q_{b_k}^w,\delta b_a,\delta b_g\end{array}\right]\\ \delta x_c^b& =\left[\begin{array}{c}\delta p_c^b,\delta q_c^b\end{array}\right]\end{array}$$

---

残差函数

1. IMU测量残差函数
记为：$r_B({\hat{z}}_{b_{k+1}}^{b_k},\mathrm{X})$，此时暂时不管其具体形式（详细推导见博客），仅从形式上明确IMU测量残差函数的自变量有两个（1）某个量测 z ^ b k + 1 b k \hat{z}^{b_k}_{b_{k+1}} z^b