Multi Sensor Fusion Based on Adaptive Kalman Filtering

Abstract

The optimal performance of the conventional Kalman filters is not guaranteed, when there is uncertainty in the process and measurement noise covariances. In this paper, in order to reduce the effect of noise covariance uncertainty, the Fuzzy Adaptive Iterated Extended Kalman Filter (FAIEKF) and Fuzzy Adaptive Unscented Kalman Filter (FAUKF) are proposed to overcome this drawback. The proposed FAIEKF and FAUKF have been applied to fuse signals from Global Positioning System (GPS) and Inertial Navigation Systems (INS) for the autonomous vehicles’ navigation. In order to validate the accuracy and convergence of the proposed approaches, results obtained by FAUKF and FAIEKF were compared to the Fuzzy Adaptive Extended Kalman Filter (FAEKF), Extended Kalman Filter (EKF), Unscented Kalman Filter (UKF), and Iterated Extended Kalman Filter (IEKF). The simulation results illustrate the superior performance of the AKUKF compared to the other filters.

Appendices

Appendix

GPS Satellite Geometry

Four pseudo range measurements are used as a measurement model of the Kalman filter.

$$ h_{1}=\sqrt{(X_{1}-x)^{2}+(Y_{1}-y)^{2}+(Z_{1}-z)^{2}}+C\varDelta t_{1} $$

$$ h_{2}=\sqrt{(X_{2}-x)^{2}+(Y_{2}-y)^{2}+(Z_{2}-z)^{2}}+C\varDelta t_{2} $$

$$ h_{3}=\sqrt{(X_{3}-x)^{2}+(Y_{3}-y)^{2}+(Z_{3}-z)^{2}}+C\varDelta t_{3} $$

$$\begin{aligned} \quad h_{4}=\sqrt{(X_{4}-x)^{2}+(Y_{4}-y)^{2}+(Z_{4}-z)^{2}}+C\varDelta t_{4} \end{aligned}$$

(31)

where, $(X_{1},Y_{1},Z_{1})$, $(X_{2},Y_{2},Z_{2})$, $(X_{3},Y_{3},Z_{3})$, $(X_{4},Y_{4},Z_{4})$ are the positions of the four GPS satellites respectively, and (x, y, z) are the position of the vehicle. The GPS satellite assumed to be in circular orbits.

$$\begin{aligned} X_{j}=R[cos \theta _{j} cos \varOmega _{i}-sin\theta _{j} sin\varOmega _{j} cos55^{\circ }] \end{aligned}$$

(32)

$$\begin{aligned} Y_{j}=R[cos \theta _{j} sin \varOmega _{j}+sin\theta _{j} sin\varOmega _{j} cos55^{\circ }] \end{aligned}$$

(33)

$$\begin{aligned} Z_{j}=R[sin \theta _{j} sin55^{\circ }] \end{aligned}$$

(34)

where

$$ \theta _{j}= \theta _{0}+T \dfrac{360}{43082}\,{\mathrm {deg}} \quad j=1,\ldots ,4 $$

$$ \varOmega _{j}=\varOmega _{0}-T \dfrac{360}{86164}\,{\mathrm {deg}} $$

$$\begin{aligned} R=26560000\,{\mathrm m} \end{aligned}$$

(35)

$$\begin{aligned} \mathbf P _{0}=\begin{bmatrix} 100&0&0&0&0&0&0&0 \\0&10&0&0&0&0&0&0\\0&0&100&0&0&0&0&0\\ 0&0&0&10&0&0&0&0\\ 0&0&0&0&100&0&0&0\\ 0&0&0&0&0&10&0&0\\ 0&0&0&0&0&0&100&0\\ 0&0&0&0&0&0&0&10\end{bmatrix} \end{aligned}$$

(36)

$$\begin{aligned} \mathbf Q = \begin{bmatrix} {{\sigma } }_{{{ x}}}^{{2}}{t}^{{3}}/{3 }&\sigma _{x}^{2}t^{2}/2&0&0&0&0&0&0 \\ {\sigma } _{x}^{2}{t}^{2}/2&{\sigma } _{x}^{2}t^{2}&0&0&0&0&0&0 \\ 0&0&{\sigma }_{{y}}^{2}t^{3}/3&\sigma _{y}^{2}t^{2}/2&0&0&0&0 \\ 0&0&{\sigma } _{y}^{2}t^{2}/2&{\sigma } _{y}^{2}t^{2}&0&0&0&0&\\ 0&0&0&0&{\sigma } _{z}^{2}t^{3}/3&{\sigma } _{z}^{2}t^{2}/2&0&0 \\ 0&0&0&0&{\sigma } _{z}^{2}t^{2}/2&{\sigma } _{z}^{2}t^{2}&0&{2}/{3}\\ 0&0&0&0&0&0&({S}_{a}t+\frac{{S}_{b}t^{3}}{3})c^{2}&\frac{S_{b}t^{2}c^{2}}{2} \\ 0&0&0&0&0&0&\frac{{S}_{b}t^{3}c^{2}}{2}&{S_{b}} \end{bmatrix} \end{aligned}$$

(37)

where, $\sigma _{x}$, $\sigma _{y}$, and $\sigma _{z}$ represent standard deviations associated with x, y, and z, respectively. t is the sample time, c is the speed of light $S_{a}=0.4(10)^{-18}$, standard deviation of clock offset, and $S_{b}=1.58(10)^{-18}$, is the standard derivation associated with velocity (Table 5).

Table 5 Satellite parameters