## 4 Vehicle State Estimation, Localization, and Sensors
### Visual-Inertial Odometry Algorithm
To propagate uncertainty of state, a discrete-time state-transition matrix extracted from Equation (10) and discrete-time covariance matrix must be calculated at the initial step as:  
\[\Phi_{k}=\Phi(t_{k+1},t_{k})=\exp\left(\int_{t_{k}}^{t_{k+1}}F(\tau)d\tau\right) \tag{11}\]  
\[Q_{k}=\int_{t_{k}}^{t_{k+1}}\Phi(t_{k+1},\tau)GQG\Phi(t_{k+1},\tau)^{T}d\tau \tag{12}\]  
where \(Q=\mathbb{E}\big{[}n_{I}\ n_{I}^{T}\big{]}\) is defined as the dispersion matrix of continuous-time noise in the system. As a result, the propagation covariance of inertial measurement unit states can be written as:  
\[P_{II_{k+1|k}}=\Phi_{k}P_{II_{k|k}}\Phi_{k}^{T}+Q_{k} \tag{13}\]  
After portioning the covariance of the overall state as:  
\[P_{k|k}=\left( \begin{array}{cc}P_{II_{k|k}}&P_{I_{C_{k|k}}}\\ P_{I_{C}k|k}^{T}&P_{C_{C_{k|k}}}\end{array} \right) \tag{14}\]  
The propagation of uncertainty can be written as:  
\[P_{k+1|k}=\left( \begin{array}{cc}P_{II_{k+1|k}}&\Phi_{k}P_{I_{C_{k|k}}}\\ P_{I_{C}k|k}^{T}\Phi_{k}^{T}&P_{C_{C_{k|k}}}\end{array} \right) \tag{15}\]  
After getting new images, the state must be augmented using renewed state from the camera. The position of the recent camera state can be calculated from the newest IMU state as follows:  
\[\bar{C}\hat{q}=\bar{C}\hat{q}\otimes\bar{C}\hat{q} \tag{16}\]  
\[\bar{C}\hat{p}c=\bar{C}\hat{p}c+C\left(\bar{C}\hat{q}\right)^{T}l\hat{p}c \tag{17}\]  
Moreover, the augmented covariance matrix is as follows:  
\[P_{k|k}=\left( \begin{array}{cc}I_{21+6N}\\ \int\end{array} \right)P_{k|k}\left( \begin{array}{cc}I_{21+6N}\\ \int\end{array} \right)^{T} \tag{18}\]  
Considering a scenario where feature \(f_{j}\) is observed using the stereo camera for position \(\left( \begin{array}{cc}C_{i}q,C\ p_{C_{i}}\end{array} \right)\), note that the used stereo cameras have two single camera cells with positions represented as \(\left( \begin{array}{cc}C_{i,1}q,C\ p_{C_{i,1}}\end{array} \right)\) and \(\left( \begin{array}{cc}C_{i,2}q,C\ p_{C_{i,2}}\end{array} \right)\) for the right side and left side camera cells in the stereo camera package, respectively. The stereo measurement, \(z_{i^{\prime}}^{j}\), is represented as:  
\[Z_{i}^{j}=\left( \begin{array}{cc}u_{i,1}^{j}\\ v_{i,1}^{j}\\ u_{i,2}^{j}\\ v_{i,2}^{j}\end{array} \right)=\left( \begin{array}{cc}\frac{1}{c_{i,1}Z_{j} }&0_{2\times 2}\\ 0_{2\times 2}&\frac{1}{c_{i,2}Z_{j}}\end{array} \right)\left( \begin{array}{cc} \begin{array}{cc}C_{i,1}X_{j}\\ C_{i,1}Y_{j}\\ C_{i,2}X_{j}\\ C_{i,2}Y_{j}\end{array}\end{array} \right) \tag{19}\]  
In Equation (19), \(\left( \begin{array}{cc}C_{i,1}X_{j}&C_{i,1}Y_{j}&C_{i,1}Z_{j}\end{array}  \right)^{T}\), \(k\in\ \{1,2\}\), are considered to be the location of the feature, \(f_{j}\), on the left-side and right-side sub-camera frame (\(C_{i,1}\), \(C_{i,2}\)) having relation to the camera location as follows:  
\[r_{i}^{j}=z_{i}^{j}-z_{i}^{j}=H_{C_{i}}^{j}\widetilde{X}_{C_{i}}+H_{f_{i}}^{j} \bar{C}\tilde{p}_{j}+n_{i}^{j} \tag{20}\]  
where \(n_{i}^{j}\) is the noise of measurement, and \(H_{C_{i}}^{j}\) and \(H_{f_{i}}^{j}\) are the measurements of the Jacobian. After collecting multiple sampled observations of the similar feature \(f_{j}\), we can have:  
\[r^{j}=H_{x}^{j}\widetilde{x}+H_{f}^{j}\bar{C}\tilde{p}_{j}+n^{j} \tag{21}\]In order to make sure that the uncertainty of \({}^{G}p_{j}\) does not have any effect on residual, the residual in (20) is projected to the kernel, \(V\), of \(H_{f}^{j}\) as follows:  
\[r_{0}^{j}=V^{T}r^{j}=V^{T}H_{x}^{j}\tilde{x}+V^{T}n^{j}=H_{x,0}^{j}\tilde{x}+n_{0 }^{j} \tag{22}\]  
Taking into account Equation (22), the updating step of EKF could be calculated. A simple execution of EKF-based VIO produces incorrect information in the heading. This problem originates from the difference between the linearizing point for process and measurement step at the same time instant. In order to maintain the consistency of the filter, a variety of different methods have been used in previous studies.