Fusing denoised stereo visual odometry, INS and GPS measurements for autonomous navigation in a tightly coupled approach

Abstract

In a GPS-denied environment, even the combination of GPS and Inertial Navigation System (INS) cannot provide location reliably and accurately. We propose a new denoised stereo Visual Odometry VO/INS/GPS integration system for autonomous navigation based on tightly coupled fusion. The presented navigation system can estimate the location of the vehicle in either GPS-denied or low-texture environments. Because of the random walk characteristics of the drift error of the inertial measurement units (IMU), the errors of the states grow with time. To correct these growing errors, a continuous update of observations is necessary. For this purpose, the system state vector is augmented with the extracted features from a stereo camera. Consequently, we utilize the measurements of extracted features from consecutive frames and GPS-derived information to make these updates. Moreover, we apply the discrete wavelet transform (DWT) technique before data fusion to improve the signal-to-noise ratio (SNR) of the inertial sensor measurements and attenuate high-frequency noises while conserving significant information like vehicle motion. To verify the performance of the proposed method, we utilize four flight benchmark datasets with top speeds of 5 m/s, 10 m/s, 15 m/s, and 17.5 m/s, respectively, collected over an airport runway by a quad rotor. The results demonstrate that the proposed VO/INS/GPS navigation system has a superior performance and is more stable than the VO/INS and GPS/INS methods in either GPS-denied or low-texture environments; it outperforms them by approximately 66% and 54%, respectively.

Appendices

Appendix 1

Rotational matrices

In continuation, the rotational matrices of the suggested system process model are given. The ${C}_{B}^{W}\left({\lambda }_{\mathrm{WB}}\left(t\right)\right)$ and $R({\lambda }_{\mathrm{WB}}\left(t\right))$ in (6) and (7) are as follows:

$$R(\lambda _{{{\text{WB}}}} (t)) = \left[ {\begin{array}{*{20}c} {\frac{{{\text{cos}}\psi }}{{{\text{cos}}\theta }}} & {\frac{{{\text{sin}}\psi }}{{{\text{cos}}\theta }}} & 0 \\ { - {\text{sin}}\psi } & {{\text{cos}}\psi } & 0 \\ {\frac{{{\text{sin}}\theta {\text{cos}}\psi }}{{{\text{cos}}\theta }}} & {\frac{{{\text{sin}}\theta {\text{sin}}\psi }}{{{\text{cos}}\theta }}} & 1 \\ \end{array} } \right]$$

(37)

$$C_{B}^{W} (\lambda _{{{\text{WB}}}} (t)) = \left[ {\begin{array}{*{20}c} {{\text{cos}}\psi {\text{cos}}\theta } & { - {\text{sin}}\psi {\text{cos}}\varphi {\text{ + cos}}\psi {\text{sin}}\theta {\text{sin}}\varphi } & {{\text{sin}}\psi {\text{sin}}\varphi {\text{ + cos}}\psi {\text{sin}}\theta {\text{cos}}\varphi } \\ {{\text{sin}}\psi {\text{cos}}\theta } & {{\text{cos}}\psi {\text{cos}}\varphi {\text{ + sin}}\psi {\text{sin}}\theta {\text{sin}}\varphi } & { - {\text{cos}}\psi {\text{sin}}\varphi {\text{ + sin}}\psi {\text{sin}}\theta {\text{cos}}\varphi } \\ { - {\text{sin}}\theta } & {{\text{cos}}\theta {\text{sin}}\varphi } & {{\text{cos}}\theta {\text{cos}}\varphi } \\ \end{array} } \right]$$

(38)

where ${\varphi },\uptheta ,$ and $\uppsi$ are the roll, pitch, and yaw angles, respectively.

Appendix 2

Jacobian matrices

In this section, the Jacobian matrices of the proposed system continuous model are computed. The ${F}_{sys}\left(t\right)$ matrix in (16) can be shown by:

$$F_{sys} (t) = \left[ \begin{gathered} F_{I} (t) \hfill \\ F_{{C_{i} }} (t) \hfill \\ \end{gathered} \right]$$

(39)

Meanwhile,

$$f_{D}^{B} (t) = f_{Denoised}^{B} (t) - b_{a} (t) + n_{a} (t)$$

(40)

$$- J(C_{B}^{W} (\lambda_{\text {WB}} (t))f_{D}^{B} = f_{D}^{B} \times C_{B}^{W} (\lambda_{\text {WB}} (t))$$

(41)

$$F_{I} (t) = \left[ {\begin{array}{*{20}c} {0_{3 \times 3} } & {I_{3} } & {0_{3 \times 3} } & {0_{3 \times 3} } & {0_{3 \times 3} } & {0_{3 \times 3N} } \\ {0_{3 \times 3} } & {0_{3 \times 3} } & { - J(C_{B}^{W} (\lambda_{\text {WB}} (t))f_{D}^{B} (t)} & {0_{3 \times 3} } & {0_{3 \times 3} } & {0_{3 \times 3N} } \\ {0_{3 \times 3} } & {0_{3 \times 3} } & {0_{3 \times 3} } & {0_{3 \times 3} } & {0_{3 \times 3} } & {0_{3 \times 3N} } \\ {0_{3 \times 3} } & {0_{3 \times 3} } & {0_{3 \times 3} } & {0_{3 \times 3} } & {0_{3 \times 3} } & {0_{3 \times 3N} } \\ {0_{3 \times 3} } & {0_{3 \times 3} } & {0_{3 \times 3} } & {0_{3 \times 3} } & {0_{3 \times 3} } & {0_{3 \times 3N} } \\ {0_{3 \times 3} } & {0_{3 \times 3} } & {0_{3 \times 3} } & {0_{3 \times 3} } & { - \frac{1}{\text{h}}I_{3} } & {0_{3 \times 3N} } \\ \end{array} } \right]$$

(42)

and

$$F_{{C_{i} }} (t) = \left[ {\begin{array}{*{20}c} {\frac{{\partial Eq_{i} .(11)(t)}}{{\partial x_{I} (t)}}} & {\frac{{\partial Eq_{i} .(11)(t)}}{{\partial x_{C} (t)}}} \\ {\frac{{\partial Eq_{i} .(12)(t)}}{{\partial x_{I} (t)}}} & {\frac{{\partial Eq_{i} .(12)(t)}}{{\partial x_{C} (t)}}} \\ {\frac{{\partial Eq_{i} .(13)(t)}}{{\partial x_{I} (t)}}} & {\frac{{\partial Eq_{i} .(13)(t)}}{{\partial x_{C} (t)}}} \\ \end{array} } \right]$$

(43)

The ${B}_{sys}\left(t\right)$ matrix in (16) can be calculated by:

$$B_{sys} (t) = \left[ {\begin{array}{*{20}c} {0_{3 \times 3} } & {0_{3 \times 3} } \\ \begin{gathered} C_{B}^{W} (\lambda_{\text {WB}} (t)) \hfill \\ 0_{3 \times 3} \hfill \\ 0_{3 \times 3} \hfill \\ 0_{3 \times 3} \hfill \\ 0_{3 \times 3} \hfill \\ 0_{3 \times 3} \hfill \\ \end{gathered} & \begin{gathered} 0_{3 \times 3} \hfill \\ B_{1} \hfill \\ 0_{3 \times 3} \hfill \\ 0_{3 \times 3} \hfill \\ 0_{3 \times 3} \hfill \\ B_{{2_{i} }} \hfill \\ \end{gathered} \\ \end{array} } \right]$$

(44)

$$B_{1} = \left[ {\begin{array}{*{20}c} 1 & {{\text{sin}}\varphi {\text{tan}}\theta } & {{\text{cos}}\varphi {\text{tan}}\theta } \\ 0 & {{\text{cos}}\varphi } & { - {\text{sin}}\varphi } \\ 0 & {{\text{sin}}\varphi {\text{sec}}\theta } & {{\text{cos}}\varphi {\text{sec}}\theta } \\ \end{array} } \right]$$

(45)

$$B_{{2_{i} }} = \left[ {\begin{array}{*{20}c} {u_{{x_{i} }} (t)u_{{y_{i} }} (t)} & {{ - }(1 + u_{{x_{i} }} (t))^{2} } & {u_{{y_{i} }} (t)} \\ {u_{{x_{i} }} (t)^{2} + 1} & {{ - }u_{{x_{i} }} (t)u_{{y_{i} }} (t)} & {{ - }u_{{x_{i} }} (t)} \\ {u_{{y_{i} }} (t)z(t)} & {{ - }u_{{x_{i} }} (t)z_{i} (t)} & 0 \\ \end{array} } \right]$$

(46)

Finally, the ${G}_{sys}\left(t\right)$ process noise matrix in Eq. (16) can be obtained as follows:

$$G_{\text {sys}} (t) = \left[ \begin{gathered} G_{I} (t) \hfill \\ G_{{C_{i} }} (t) \hfill \\ \end{gathered} \right]$$

(47)

$$G_{I} (t) = \left[ {\begin{array}{*{20}c} {0_{3 \times 3} } & {0_{3 \times 3} } & {0_{3 \times 3} } \\ {C_{B}^{W} (\lambda_{\text{{{WB}}}} (t))} & {0_{3 \times 3} } & {0_{3 \times 3} } \\ {0_{3 \times 3} } & {C_{B}^{W} (\lambda_{\text{{{WB}}}} (t))} & {0_{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} } \\ {0_{3 \times 3} } & {0_{3 \times 3} } & {I_{3} } \\ \end{array} } \right]$$

(48)

and

$$G_{{C_{i} }} (t) = \left[ {\begin{array}{*{20}c} {0_{3 \times 3} } & {u_{{x_{i} }} (t)u_{{y_{i} }} (t)} & {{ - }(1 + u_{{x_{i} }}^{2} (t))} & {u_{{y_{i} }} (t)} & {0_{3 \times 3} } \\ {0_{3 \times 3} } & {1 + u_{{y_{i} }}^{2} (t)} & {{ - }u_{{x_{i} }} (t)u_{{y_{i} }} (t)} & {{ - }u_{{x_{i} }} (t)} & {0_{3 \times 3} } \\ {0_{3 \times 3} } & {{\upzeta }_{{\text{i}}} {\text{(t)}}u_{{y_{i} }} (t)} & {{ - }{\upzeta }_{{\text{i}}} {\text{(t)}}u_{{x_{i} }} (t)} & {0_{1 \times 1} } & {0_{3 \times 3} } \\ \end{array} } \right]$$

(49)

where the value of $i$ depends on the number of the detected features ($\mathrm{N}$) in the phase of the initialization. Also, $\times$ in (41) represents the cross product.