## 6 EKF Modeling and Implementation  
To optimally implement sensor integration, several forms of non-linear filtering techniques were previously developed and validated, such as the EKF, unscented Kalman filter (UKF) as well as particle filter (PF). An EKF design linearizes the system and measurement models by considering a first-order Taylor series expansion at the predicted state estimate. This first-order approximation enables the linearized models to implement standard Kalman filter processing. In a typical UKF design, particularly for high non-linear instances, the selected sigma points through the unscented transform estimate the mean and covariance of the state vector. Based on the Bayesian estimation theory, the PF design is supposed to achieve a better solution compared to the EKF and UKF implementations if it is properly established. Nevertheless, the intense computational burden of the UKF and PF processing, one of their prominent shortcomings, brings about severe time-lag estimation and fatally hinders real-time road applications. On the other hand, for low-dynamic applications such as consumer-grade ground vehicle navigation, a time-efficient EKF-based sensor integration scheme requires limited computation and can agree well with the actual situation in terms of the accuracy in statistics [35, 36]. The EKF approach is thus adopted as the proposed filter design. As described previously, Equations (4) to (9) serve as the EKF discretized continuous-time model which is summarized as:  
\[\frac{dx(t)}{dt}=\mathrm{Fx}(t)+\mathrm{Gw}(t),\ \mathrm{w}(t){\sim}N(0, \mathrm{Q}(t)) \tag{18}\]  
In Equation (18), F and G are the linearized dynamic matrix and input coefficient matrix with constant dimensionalities. Additive white Gaussian noise of the system is denoted as \(\mathrm{w}(t)\) with a power spectral density \(\mathrm{Q}(t)\) and given by:  
\[\mathrm{w}(t)=[w_{f\,3\times 1}^{T}\ w_{\omega}^{T}\ _{3\times 1}\ \mu_{f\,3 \times 1}^{T}\ \mu_{\omega\,3\times 1}^{T}]^{T} \tag{19}\]  
where the four triads denote uncorrelated and correlated noises of the sensor biases. The 15-element system state vector is given by:  
\[\mathrm{x}(t)=[\delta P_{3\times 1}^{n}\ \delta V_{3\times 1}^{n}\ \delta\Psi_{3\times 1}^{n}\ \delta f_{3\times 1}^{b}\ \delta\omega_{3\times 1}^{b}]^{T} \tag{20}\]  
where the triad perturbation terms of position, velocity, orientation and sensor biases are included. As camera-derived navigation parameters, the rotation matrix \(R_{c}(t)\) and translation unit vector \(\vec{l}_{c}(t)\) require additional transformations to obtain the camera-based position and orientation under the \(e\) and \(n\) frames, correspondingly. Let \(C_{b}^{n}(0)\) be the initial IMU orientation matrix and \(C_{c}^{b}\) be the constant rotation matrix between the \(c\) and \(b\) frames. Assuming the origins of the \(c\) and \(b\) frames coincide, the camera-based orientation \(\tilde{C}_{b}^{n}(t)\) can be expressed as:  
\[\tilde{C}_{b}^{n}(t)=C_{b}^{n}(0)\cdot C_{c}^{b}\cdot(V(t))^{T}\cdot(C_{c}^{b} )^{T} \tag{21}\]  
where \(V(t)\) is the accumulated \(3\times 3\) camera rotation from the first image and satisfies:  
\[V(t)=R_{c}(t)\cdot V(t-1),\ V(1)=R_{c}(1) \tag{22}\]  
Assuming \(\tilde{C}_{b}^{n}(t)\) contains only WGN, the orientation error term \(\delta\Psi^{n}\) in Equation (20) correlates \(\tilde{C}_{b}^{n}(t)\) as well as IMU-based orientation \(C_{b}^{n}\) with the relationship of:  
\[\tilde{C}_{b}^{n}(t)=[l_{3\times 3}+(\delta\Psi^{n}(t)\times)]\cdot C_{b}^{n}(t )+\Gamma_{3\times 3}(t) \tag{23}\]  
where matrix \(\Gamma_{3\times 3}\) consists of the measurement noises.