## 4 Vehicle State Estimation, Localization, and Sensors
### Visual-Inertial Odometry Algorithm  
In this study, a Kalman-filter-based stereo visual inertial odometry is used. Data from the IMU and a stereo camera are used in the algorithm. The IMU model can be written as:  
\[\mathbf{X}_{I}\ =\ \begin{pmatrix}\hat{C}q^{T},\mathbf{b}_{S}^{T},\ \hat{C}_{p}^{T},\mathbf{b}_{ a}^{T},\ \hat{C}_{p}^{T},\hat{C}_{p}^{T},\hat{C}q^{T},\ \hat{C}_{p}^{T}\end{pmatrix} ^{T} \tag{1}\]  
Figure 1: Controller and localization system architecture. where \({}_{G}^{L}\mathbf{q}^{T}\) (\(q\) stands for quaternion) provides the rotation from the inertial frame to the vehicle frame, \({}^{G}\mathbf{v}_{I}\) and \({}^{G}\mathbf{p}_{I}\) define the linear velocity and location of the vehicle frame mapped into the inertial frame, respectively, arrays \(\mathbf{b}_{g}\) and \(\mathbf{b}_{a}\) are defined as measurement biases of velocity and acceleration from the inertial measurement unit, respectively, and \({}_{C}^{I}\mathbf{q}\) and \({}_{D}^{I}\mathbf{p}_{C}\) provide the transformation between the body and camera frames, respectively. In order to avoid singularities in covariance matrixes, the IMU error-states in Equation (1) can be modified as follows:  
\[\widetilde{\mathbf{X}}_{I} = \left({}_{G}^{L}\widetilde{\mathbf{\theta}}^{T},\widetilde{\mathbf{b}}_{g} ^{T},\widetilde{\mathbf{v}}_{I}^{T},\widetilde{\mathbf{b}}_{a}^{T},\widetilde{\mathbf{v}} _{I}^{T},\widetilde{\mathbf{\theta}}^{T},{}_{C}^{T}\right)^{T} \tag{2}\]  
In this relation, standard additive error is used for position, velocity, and biases (e.g., \({}^{G}\widetilde{\mathbf{p}}_{I}={}^{G}\mathbf{p}_{I}-{}^{G}\hat{\mathbf{p}}_{I}\)). The quaternion error, \(\delta q=q\otimes\hat{q}^{-1}\), has a close relation to state error as follows:  
\[\delta_{q}\approx\left(0.5\,{}_{I}^{G}\widetilde{\mathbf{\theta}}^{T},1\right)^{T} \tag{3}\]  
where, \({}_{I}^{G}\widetilde{\mathbf{\theta}}\) is a representation of a small angle rotation. As a result, the ultimate state error can be written as:  
\[\widetilde{\mathbf{X}}=\left(\widetilde{\mathbf{X}}_{I}^{T},\widetilde{\mathbf{X}}_{C_{1} }^{T},\,\ldots,\widetilde{\mathbf{X}}_{C_{N}}^{T}\right)^{T} \tag{4}\]  
where each camera state error can be described as follows:  
\[\widetilde{\mathbf{X}}_{C_{i}}=\left({}_{G}^{C}\widetilde{\mathbf{\theta}}^{T},{}^{G} \widetilde{\mathbf{p}}_{C_{i}}^{T}\right)^{T} \tag{5}\]  
In order to obtain a process model, an indiscrete dynamic model of the estimated inertial measurement unit states can be considered as:  
\[ \begin{array}{l}{}_{C}^{I}\dot{q}\ =0.5\Omega(\hat{\omega})_{C}^{I}\dot{q},\ \dot{b}_{g}=0_{3\times 1}\\ \\ {}_{\hat{\psi}}=C\left({}_{C}^{I}\hat{q}\right)^{T}\hat{a}+G_{g}\\ \\ {}_{B}^{I}=0_{3\times 1},\ {}^{G}\hat{\mathbf{p}}_{I}={}^{G}\hat{V},\\ \\ {}_{C}^{I}\dot{q}=0_{3\times 1},\ {}^{I}\hat{\mathbf{p}}_{C}=0_{3\times 1}\end{array}  \tag{6}\]  
where \(\hat{\omega}\) and \(\hat{a}\) are extracted from the IMU measurements for angular velocity and acceleration (without biases) as follows:  
\[\hat{\omega}=\omega_{m}-\dot{b}_{g} \tag{7}\]  
\[\hat{a}=a_{m}-\dot{b}_{a} \tag{8}\]  
However,  
\[\Omega(\hat{\omega})=\left( \begin{array}{cc}-[\hat{\omega}_{X}]&\omega\\ -\omega^{T}&0\end{array} \right) \tag{9}\]  
where \([\hat{\omega}_{\times}]\) is the antisymmetric matrix of \(\hat{\omega}\) at Equation (6) playing a role as quaternion to the rotation matrix convertor. According to Equation (6), the linearized indiscrete dynamics for the IMU state error can be as follows:  
\[\dot{\widetilde{X}}_{I}=F\widetilde{x}_{I}+Gn_{I} \tag{10}\]  
Here, \(n_{I}^{T}=\left(n_{g}^{T},n_{ug}^{T},n_{a}^{T},n_{au}^{T}\right)^{T}\). The vectors \(n_{g}\) and \(n_{a}\) are representations of a Gaussian noise of gyroscope measurement and accelerometer. The other terms \(\left(n_{\omega g}\) and \(n_{\omega u}\right)\) represent the random walk rate of the gyroscope and accelerometer measurement biases.