# Visual-Inertial Navigation System Based on Virtual Inertial Sensors
## 4 Analysis and Experiments of Visual-Inertial Odometry Optimization
### Analysis of VINS Odometry Optimization
\begin{array}{l}\begin{bmatrix}\hat{\mathbf{a}}_{b_{k+1}}^{b_{k}}\\ \hat{\mathbf{p}}_{b_{k+1}}^{b_{k}}\\ \hat{\mathbf{p}}_{b_{k+1}}^{b_{k}}\\ \hat{\mathbf{p}}_{b_{k+1}}^{b_{k}}\\ 0\end{bmatrix}=\begin{bmatrix}R_{\omega}^{b_{k}}(P_{b_{k+1}}^{\omega}-P_{b_{k}} ^{\omega}+\frac{1}{2}g^{\omega}\Delta t_{k}^{2}-v_{b_{k}}^{\omega}\Delta t_{k })\\ R_{\omega}^{b_{k}}(v_{b_{k+1}}^{\omega}+g^{\omega}\Delta t_{k}-v_{b_{k}}^{ \omega})\\ q_{b_{k}}^{-1}\otimes q_{b_{k+1}}^{\omega}\\ b_{a_{b_{k+1}}}-b_{a_{b_{k}}}\\ b_{a_{b_{k+1}}}-b_{a_{b_{k}}}\end{bmatrix}\end{array}  \tag{3}\]  
Considering the measurements of the foot-mounted VIMU within a sliding window comprising consecutive frames \(b_{k}\) and \(b_{k+1}\), the residual of the pre-integrated foot-mounted VIMU measurement can be formulated in accordance with the defined VIMU measurement model:  
\[r_{VB}(g_{b_{k+1}}^{b_{k}},X)= \begin{bmatrix}\delta a_{b_{k+1}}^{b_{k}}\\ \delta b_{b_{k+1}}^{b_{k}}\\ \delta b_{b_{k+1}}^{b_{k}}\\ \delta b_{b_{k+1}}^{b_{k}}\\ \delta b_{b_{k}}\end{bmatrix} = \begin{bmatrix}R_{\omega}^{b_{k}}(P_{b_{k+1}}^{ \omega}-P_{b_{k}}^{\omega}+\frac{1}{2}g^{\omega}\Delta t_{k}^{2}-v_{b_{k}}^{ \omega}\Delta t_{k})-\hat{a}_{b_{k+1}}^{b_{k}}\\ R_{\omega}^{b_{k}}(v_{b_{k+1}}^{\omega}+g^{\omega}\Delta t_{k}-v_{b_{k}}^{ \omega})-\hat{\beta}_{b_{k+1}}^{b_{k}}\\ 2\left[q_{b_{k}}^{\omega-1}\otimes q_{b_{k+1}}^{\omega}\otimes\left(\hat{ \gamma}_{b_{k+1}}^{b_{k}}\right)^{-1}\right]_{xyz}\\ b_{a_{b_{k+1}}}-b_{a_{b_{k}}}\\ b_{\omega_{b_{k+1}}}-b_{a_{b_{k}}}\end{bmatrix}  \tag{4}\]  
Figure 6: Flowchart of localization and mapping process of VINS. The system utilizes an iterative nonlinear least squares optimization method to minimize the cost function and update the estimated camera pose, the IMU and VIMU states. The detailed optimization process and related formulas are presented in the literature [21]. By adding ZUPT constraints into the cost function, the enforcement of zero velocity during stationary periods is facilitated, leading to enhanced overall pose estimation accuracy and improved smoothness of visual-inertial odometry. The ZUPT error terms for the velocity and camera pose during stationary periods can be formulated as follows:  
\[ \begin{array}{c}error_{\text{ZUPT}}^{2}=K_{v}\cdot(v_{pred}-v_{obs})\\ error_{\text{ZUPT}}^{p}=K_{p}\cdot(p_{pred}-p_{obs})\end{array}  \tag{5}\]  
where \(v_{pred}\) and \(p_{pred}\) are the predicted velocity and position from IMU integration, \(v_{obs}\) and \(p_{obs}\) are the observed velocity and position from the ZUPT constraint, and \(K_{v}\) and \(K_{p}\) are the gain factor that controls the influence of the ZUPT constraint on the velocity and camera pose estimation. The ZUPT error terms for the velocity and camera pose during stationary periods can be included as additional error terms in the cost function for the optimization step. The additional ZUPT constraint can be formulated as  
\[error_{\text{ZUPT}}=\lambda_{v}error_{\text{ZUPT}}^{p}+\lambda_{p}error_{ \text{ZUPT}}^{p} \tag{6}\]  
where \(\lambda_{v}\) and \(\lambda_{p}\) are regularization parameters that control the trade-off between the other measurements and the ZUPT constraints, and \(error_{\text{ZUPT}}^{p}\) and \(error_{\text{ZUPT}}^{p}\) are the ZUPT error terms for velocity and camera pose, respectively. The incorporation of zero-velocity constraints provides additional information to the optimization process, helping to improve the accuracy of estimating the camera pose and the IMU and VIMU states during stationary periods when visual measurements may be ambiguous or noisy. Additionally, by providing additional constraints on the system's motion, it helps improve the consistency of the estimated trajectory, reducing drift and improving the long-term stability of the estimated trajectory. This article shows the adjustments made to the original state estimation framework of VINS, replacing the original IMU with a ZUPT-modified VIMU and incorporating ZUPT constraints.