Fuzzy adaptive extended Kalman filter for UAV INS/GPS data fusion

Abstract

This paper uses the extended Kalman filter (EKF) for estimation of position, velocity and attitude of an UAV of quadrotor type. The sensors used are low cost microelectromechanical systems (MEMS) accelerometer and gyroscope, MEMS barometer and GPS. The standard EKF is improved with adaptive approaches. Two innovation adaptive estimation methods were taken from the literature. One involves the change of the theoretical matrix of the innovation covariance, by another one which is determined experimentally; the second one involves the computation of the output noise covariance matrix of the Kalman filter (KF) via fuzzy logic. A new method for adaptive estimation was also developed by extending an approach of heuristic metrics of bias and amplitude of oscillation taken from the literature. It consists in the use of the metrics, together with fuzzy logic, for adjusting the input and output noise covariance matrices of the EKF. The adaptive and standard filters were both implemented in hardware and simulated. The hardware implementation involves rotation filters in indoor tests performed in open loop; these observations were exclusively carried out in a hand-controlled environment, with no flight of the quadrotor itself. The simulations were performed in conditions of the full capability of the filters, in a scenario representing a fault condition in the gyroscope. The tests showed the ability of the adaptive filters of correcting the covariance matrix for improving performance.

Appendix

1.1 Kalman filter: Notation and structure

The EKF considers the nonlinear dynamical system represented by the model in Eq. 30.

$$\begin{aligned} {\dot{\mathbf {x}}}(t)={\mathbf {f}}({\mathbf {x}},{\mathbf {u}}(t),t)+{\mathbf {G}}({\mathbf {x}},t){\mathbf {w}}(t) \end{aligned}$$

(30)

where ${\mathbf {x}}\in {\mathbb {R}}^n$ is the state, ${\mathbf {u}}: [0,\ \infty )\rightarrow {\mathbb {R}}^l$ is a given driving function, ${\mathbf {f}}: {\mathbb {R}}^n\times {\mathbb {R}}^l\times [0,\ \infty )\rightarrow {\mathbb {R}}^n$ is a vector valued nonlinear state function. ${\mathbf {w}}\in {\mathbb {R}}^m$ is called process noise, it is a white Gaussian process with zero mean and covariance ${\mathbf {Q}}(t)\in {\mathbb {R}}^{m\times m}$. ${\mathbf {G}}: {\mathbb {R}}^n\times [0,\ \infty )\rightarrow {\mathbb {R}}^{n\times m}$ is a matrix valued nonlinear function called process noise distribution matrix.

The measurements are state dependent and represented by the nonlinear output function in Eq. 31.

$$\begin{aligned} {\mathbf {z}}(t)={\mathbf {h}}({\mathbf {x}})+{\mathbf {v}}(t), \end{aligned}$$

(31)

where ${\mathbf {z}}\in {\mathbb {R}}^p$ is the vector of measured outputs, ${\mathbf {h}}: {\mathbb {R}}^n\rightarrow {\mathbb {R}}^p$ is a vector-valued nonlinear output function. ${\mathbf {v}}\in {\mathbb {R}}^p$ is called measurement noise, it is a white Gaussian process with zero mean and covariance ${\mathbf {R}}(t)\in {\mathbb {R}}^{p\times p}$.

The following procedure composes the k-th iteration of the EKF. An initial estimate is needed, as usual, the method assumes that the initial state is distributed according to a white Gaussian noise with zero mean.

1.1.1 Prediction (time propagation)

Prediction of the state vector:

$$\begin{aligned} {\hat{\mathbf {x}}}_{k+1}^-={\hat{\mathbf {x}}}_{k}^++\int _{t_{k}}^{t_{k+1}}{\mathbf {f}}({\mathbf {x}}(t),{\mathbf {u}}(t),t){\rm{d}}t \end{aligned}$$

(32)

Prediction of the error’s covariance matrix:

$$\begin{aligned} {\dot{\mathbf {P}}}(t)={\mathbf {F}}_k{\mathbf {P}}(t)+{\mathbf {P}}(t){\mathbf {F}}_k^t+{\mathbf {Q}}_k^{''} \end{aligned}$$

(33)

$$\begin{aligned} {\mathbf {F}}_k=\left. \frac{\partial {\mathbf {f}}}{\partial {\mathbf {x}}}\right| _{{\mathbf {x}}={\hat{\mathbf {x}}}_{k}^+} \end{aligned}$$

(34)

$$\begin{aligned} {\mathbf {Q}}_k^{''}={\mathbf {G}}({\hat{\mathbf {x}}}_{k}^+,t_k){\mathbf {Q}}(t_k){\mathbf {G}}^t({\hat{\mathbf {x}}}_{k}^+,t_k) \end{aligned}$$

(35)

$$\begin{aligned} {\mathbf {P}}_{k+1}^-={\mathbf {P}}_{k}^++\int _{t_{k}}^{t_{k+1}}{\dot{\mathbf {P}}}(t){\rm{d}}t \end{aligned}$$

(36)

1.1.2 Correction (measurement update)

Kalman gain:

$$\begin{aligned} {\bar{\mathbf {K}}}_k={\mathbf {P}}_{k+1}^-{\mathbf {H}}_k^t\left[ {\mathbf {H}}_k{\mathbf {P}}_{k+1}^-{\mathbf {H}}_k^t+{\mathbf {R}}_k \right] ^{-1} \end{aligned}$$

(37)

$$\begin{aligned} {\mathbf {H}}_k=\left. \frac{\partial {\mathbf {h}}}{\partial {\mathbf {x}}}\right| _{{\mathbf {x}}={\hat{\mathbf {x}}}_{k+1}^-} \end{aligned}$$

(38)

Correction of the state estimate:

$$\begin{aligned} {\hat{\mathbf {x}}}_{k+1}^+={\hat{\mathbf {x}}}_{k+1}^-+{\bar{\mathbf {K}}}_k\left[ {\mathbf {z}}_{k}-{\mathbf {h]}({\hat{\mathbf {x}}}}_{k+1}^-)\right] \end{aligned}$$

(39)

Correction of the error’s covariance matrix estimate:

$$\begin{aligned} {\mathbf {P}}_{k+1}^+={\mathbf {P}}_{k+1}^--{\bar{\mathbf {K}}}_k{\mathbf {H}}_k{\mathbf {P}}_{k+1}^- \end{aligned}$$

(40)

In Eq. 32, the vector function ${\mathbf {u}}(t)$ is given by data determined from measurement, for example, from a gyroscope. In Eq. 39, ${\mathbf {z}}_k$ is the output measured by another instrument at time k, for example, a GPS receiver.