## 6 EKF Modeling and Implementation
Rearranging Equation (23) yields:  
\[\delta\Psi^{n}(t)\times=(\tilde{C}_{b}^{n}(t)-C_{b}^{n}(t))\cdot(C_{b}^{n}(t ))^{-1}+\Gamma_{3\times 3}(t) \tag{24}\]  
Let \(a_{3,2}\), \(a_{1,3}\) and \(a_{2,1}\) be the corresponding elements from the matrix of right-hand side multiplication and construct:  
\[\mathrm{A}=[a_{3,2}\ \ a_{1,3}\ \ a_{2,  
which means a unit vector expressed in the \(n\) frame. Note that the orientation matrix \(\hat{C}_{b}^{n}\) does not refer to the present moment but the instant of previous image update and the caret \({}^{\wedge}\) indicates the EKF-based estimation. By obtaining the translation magnitude \(\delta\mathrm{R}_{r}\) which has been resolved in Section 5 by using differential LOS GNSS pseudoranges over epochs, the camera-based position is expressed as:  
\[\tilde{P}^{e}(t)=\hat{P}^{e}(t-1)+\delta\mathrm{R}_{r}(t)\cdot l_{n}(t) \tag{29}\]  
In the above equation, \(\tilde{P}^{e}(t)\) depicts the camera-based position at the present moment, and \(\hat{P}^{e}(t-1)\) indicates the previous EKF position update, both expressed in the \(e\) frame. By subtracting the IMU-based position \(P^{e}(t)\) from \(\tilde{P}^{e}(t)\), the observation equation with respect to the position is established by:  
\[P^{e}(t)-\tilde{P}^{e}(t)=\delta P^{n}(t)+\zeta(t) \tag{30}\]  
where \(\zeta(t)\) denotes the measurement noise. Combining Equations (27) and (30) yields the EKF measurement model as below:  
\[\mathrm{Y}(t)=\mathrm{Hx}(t)+\mathrm{v}(t),\ \mathrm{v}(t){\sim}N(0,\mathrm{R}_{ cov}(t)) \tag{31}\]  
In Equation (31), the measurement and noise vectors are respectively 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}\]0  
\[\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}\]1  
\(\mathrm{v}(t)\) is assumed as WGN vector with a measurement noise covariance \(\mathrm{R}_{cov}(t)\). The measurement model matrix \(\mathrm{H}\) is given by:  
\[\mathrm{H}=[ \begin{array}{cccc}I_{3\times 3}&0_{3\times 3}&0_{3\times 3}&0_{3 \times 3}&0_{3\times 3}\\ 0_{3\times 3}&0_{3\times 3}&I_{3\times 3}&0_{3\times 3}&0_{3\times 3}\end{array} ] \tag{34}\]  
Following the standard EKF procedure, the state vector as well as its covariance matrix can be estimated and applied to ground vehicle navigation. After each update cycle, the state vector is reset to zero and refreshed by the next update operation. Figure 5 depicts an overall flowchart for integrating the camera, IMU and GNSS sensors through the EKF engine. The sensor units are colored cyan while white blocks indicate the utilized algorithms for intermediate parameter calculation. Between camera/GNSS data update, the vehicle motion is estimated by strapdown IMU mechanization. Once new camera/GNSS measurement data arrive, all derived navigation information is transformed in order to enable the EKF engine for accurate navigation solution and IMU sensor calibration. In such a method, the measurement model is straightforward to construct as relatively small/constant size of measurement elements are included in the EKF. The whole processing requires a relatively low computational load since covariance matrices have low dimensionality. Finally, a low-pass filter before implementing the EKF can be added to smooth the camera-derived rotation and translation parameters.