Features of Invariant Extended Kalman Filter Applied to Unmanned Aerial Vehicle Navigation

Abstract

This research used an invariant extended Kalman filter (IEKF) for the navigation of an unmanned aerial vehicle (UAV), and compared the properties and performance of this IEKF with those of an open-source navigation method based on an extended Kalman filter (EKF). The IEKF is a fairly new variant of the EKF, and its properties have been verified theoretically and through simulations and experiments. This study investigated its performance using a practical implementation and examined its distinctive features compared to the previous EKF-based approach. The test used two different types of UAVs: rotary wing and fixed wing. The method uses sensor measurements of the location and velocity from a GPS receiver; the acceleration, angular rate, and magnetic field from a microelectromechanical system-attitude heading reference system (MEMS-AHRS); and the altitude from a barometric sensor. Through flight tests, the estimated state variables and internal parameters such as the Kalman gain, state error covariance, and measurement innovation for the IEKF method and EKF-based method were compared. The estimated states and internal parameters showed that the IEKF method was more stable and convergent than the EKF-based method, although the estimated locations, velocities, and altitudes of the two methods were comparable.

1. Introduction

The invariant extended Kalman filter is a fairly recent development that improves the extended Kalman filter using the geometrically transformed state error and output error based on the Lie group theory [1]. It could be regarded as an extension and generalization of the multiplicative extended Kalman filter [2,3,4,5]. It can be derived and applied using either the abstract Lie group [6] or matrix Lie group formulation [7]. Derivations of the invariant extended Kalman filter (IEKF) for use in the navigation of an unmanned aerial vehicle using measurements of the location and velocity from a GPS; the angular rate, acceleration, and magnetic field from a microelectromechanical system-attitude heading reference system (MEMS-AHRS); and the altitude from a barometric sensor have been reported [2,8]. These were based on the abstract Lie group approach. A matrix Lie group formulation of the IEKF was used for simultaneous localization and mapping (SLAM) and improved the consistency of the estimation compared to the EKF-based SLAM method [9].

The IEKF approach has several distinctive advantageous features compared to the usual EKF-based approaches: (1) a symmetry preserving observer, (2) sound geometric structures for quaternion estimation, and (3) a large expected domain of convergence [8]. The IEKF-based method preserves the norm of the estimated quaternion as a unit. In contrast, the prediction and correction procedures of the usual EKF break the unit norm constraint on the quaternion for the attitude. In addition, the approximation to a linear system of a nonlinear process and measurement model sometimes induces an unstable and divergent estimation. The IEKF relieves this problem by extending the state space where convergence is preserved.

The IEKF is basically an invariant observer which adopts an EKF approach for determining observer gain to take advantage of the convergence property of the linear Kalman filter. The property of invariant, or otherwise called symmetry-preserving system had been used for control applications [10]. Aghannan and Rouchon [11] initially exploited the invariance property for observer or estimator design resulting in an invariant observer. Though the invariant observer proposed in [11] was the predecessor of the IEKF, it did not incorporate the idea of EKF. The invariant observer was further developed and convergence property was verified by numerical simulation using a mechanical system of ball and beam [12]. Martin et al. introduced invariant output error in their development of an invariant tracking method [13]. Though the invariant output error was proposed and used for control use, it played an important role in development of IEKF later [14]. The invariant observer was theoretically further investigated, providing the foundations for development into IEKF, especially for navigation applications [15]. Finally, IEKF was proposed by incorporating the EKF methodology into the invariant observer for determining the observer gain [14]. It should be noticed that the IEKF does not use direct linearization of nonlinear process model and measurement model, unlike the usual EKF [16]. This will be discussed in Section 3.1 along with the explanation about the linearization process.

Studies on the IEKF have demonstrated the feasibility of using it for aerial, indoor, and road navigation [17]. The IEKF and EKF were applied for SLAM, and a comparison of their results proved that the IEKF improved the estimation convergence and consistency compared to the EKF [18]. The comparison used simulated data. The IEKF was also used for helicopter unmanned aerial vehicle (UAV) navigation [19], and its estimation performance was found based on experimental results, focusing on the estimated data of the state variables, excluding internal parameters such as the Kalman gain, state error covariance, and measurement innovation. The IEKF was also compared with the multiplicative extended Kalman filter (MEKF) [2,8,20]. Barczyk et al. compared the IEKF with the MEKF for indoor localization using scan matching point clouds captured by a low-cost Kinect depth camera [20]. They used experimental results and compared the Kalman gain for the case of indoor navigation. Barczy and Lynch applied the IEKF to a magnetometer-plus-GPS-aided inertial navigation system for a helicopter UAV, and presented experimental data on the estimated state variables [21].

This study concentrated on supporting the availability of IEKF navigation by a comparison and analysis of the estimation results from flight tests of UAVs, which are widely used. Experiments using two UAVs, a fixed wing UAV and a rotary wing UAV, compared the features of the IEKF and conventional EKF. In particular, this study compares the internal parameters of the Kalman gain, state error covariance, and measurement innovation to explicitly show the features of the IEKF. Theoretical derivations and simulations have verified the features of the IEKF in numerous studies [1,6,7,8]. However, studies investigating and analyzing the estimation results and internal parameters through practical applications were not sufficient to validate the feasibility of using the IEKF in a wide range of applications. In studies where experimental results were compared, they usually concentrated on the estimated results of state variables, excluding internal parameters such as the Kalman gain, state error covariance, and measurement innovation [19].

Section 2 lists the notations used to describe the IEKF-based navigation methods. Section 3 briefly describes the IEKF-based navigation method used in this research, and also explains the EKF-based method that was used for comparison. Section 4 provides details of the experiments using a fixed wing UAV and quadrotor to verify and compare the methods. Section 5 compares and discusses the properties revealed from the experiments. This section compares the estimation results for the state variables and internal parameters such as the Kalman gain, state error covariance, and measurement innovation. Finally, Section 6 provides some concluding remarks and possible future research directions related to the current research.

2. Nomenclature

The following nomenclature is used in the paper.

$\mathbf{q}\left(t\right)$	attitude represented by quaternion at time t: $\mathbf{q}\left(t\right)={\left(q_0\left(t\right),q_x\left(t\right),q_y\left(t\right),q_z\left(t\right)\right)}^T$
	where $q_0\left(t\right)$ is the scalar part and $(q_x\left(t\right),q_y\left(t\right),q_z\left(t\right))$ is the vector part of the quaternion
$\mathbf{x}\left(t\right)$	location at time t: $\mathbf{x}\left(t\right)={\left(x\left(t\right),y\left(t\right),z\left(t\right)\right)}^T$
$\mathbf{v}\left(t\right)$	velocity at time t: $\mathbf{v}\left(t\right)=\frac{d\mathbf{x}\left(t\right)}{dt}={\left(v_x\left(t\right),v_y\left(t\right),v_z\left(t\right)\right)}^T$
$\mathit{\omega }\left(t\right)$	rotational velocity measured by gyroscope in the instrument coordinate frame at time t:
	$\mathit{\omega }\left(t\right)={\left({\omega }_x\left(t\right),{\omega }_y\left(t\right),{\omega }_z\left(t\right)\right)}^T$
$\mathbf{a}\left(t\right)$	acceleration measured in the instrument coordinate frame at time t:
	$\mathbf{a}\left(t\right)={(a_x\left(t\right),a_y\left(t\right),a_z\left(t\right))}^T$
${\mathbf{z}}_{\mathbf{x}}\left(t\right)$	measurement of location at time t: ${\mathbf{z}}_{\mathbf{x}}\left(t\right)={(z_x\left(t\right),z_y\left(t\right),z_z\left(t\right))}^T$
${\mathbf{z}}_{\mathbf{v}}\left(t\right)$	measurement of velocity at time t: ${\mathbf{z}}_{\mathbf{v}}\left(t\right)={(z_{v_x}\left(t\right),z_{v_y}\left(t\right),z_{v_z}\left(t\right))}^T$
${\mathbf{z}}_{\mathbf{m}}\left(t\right)$	measurement of magnetic field in the instrument coordinate frame at time t:
	${\mathbf{z}}_{\mathbf{m}}\left(t\right)={(z_{m_x}\left(t\right),z_{m_y}\left(t\right),z_{m_z}\left(t\right))}^T$
$z_h\left(t\right)$	measurement of altitude at time t
$b_h\left(t\right)$	bias in altitude measurement at time t: $z_h\left(t\right)=z\left(t\right)+b_h\left(t\right)$
${\mathbf{b}}_{\mathit{\omega }}\left(t\right)$	bias in angular rate measurement at time t: ${\mathbf{b}}_{\mathit{\omega }}\left(t\right)={\left(b_{{\omega }_x}\left(t\right),b_{{\omega }_y}\left(t\right),b_{{\omega }_z}\left(t\right)\right)}^T$
$s_{\mathbf{a}}\left(t\right)$	scale factor in acceleration measurement at time t
$\mathbf{g}$	gravitational acceleration regarded as constant throughout the implementation
	in the northeast down (NED) coordinate system
$\mathbf{m}$	geomagnetic field appropriate for a location in the NED coordinate frame that is considered
	to be constant throughout the experiment: $\mathbf{m}={(m_x,m_y,m_z)}^T$
${\mathbf{e}}_d$	$3\times 1$ unit vector in the direction d, where $d\in \{x,y,z\}$

In the following description, time index $\left(t\right)$ will be omitted from the notations if there arises no ambiguity without index $\left(t\right)$ in the context. Since the AHRS measures acceleration and angular rate as well as the magnetic field, it is also considered that AHRS includes inertial measurement unit (IMU). Thus, in this paper, IMU measurement means the acceleration and angular rate measured by AHRS.

3. IEKF and ecl-EKF for UAV Navigation

This section briefly describes the IEKF-based navigation method adopted in this research and the EKF-based method that was used for comparison. Two IEKF methods were used: one for estimating the location, attitude, and velocity [2], and the other for estimating the attitude and velocity [8]. Both of them specifically used right IEKF methods and were from [2,8], respectively. The method to be compared with the IEKF methods was the EKF from the estimation and control library (ECL) of the PX4 project [22], which will be called the ecl-EKF from now on. Hereafter, the IEKF used for estimating the location, attitude, and velocity will be called the IEKF-lav, while the IEKF for the attitude and velocity will be called the IEKF-av.

3.1. IEKF-lav: Right IEKF for Estimation of Location, Attitude, and Velocity

The right IEKF used for estimating the attitude, velocity, and location is first described [2]. The state vector $\mathit{\chi }$ to be estimated consists of the state variables shown in Equation (1):

$$\mathit{\chi }={(\mathbf{q},\mathbf{v},\mathbf{x},{\mathbf{b}}_{\mathit{\omega }},s_{\mathbf{a}},b_h)}^T.$$

(1)

The state is subject to the process model shown as a differential Equation (2):

$$\dot{\mathit{\chi }}=\left(\begin{array}{c}\dot{\mathbf{q}}\\ \dot{\mathbf{v}}\\ \dot{\mathbf{x}}\\ {\dot{\mathbf{b}}}_{\omega }\\ {\dot{s}}_{\mathbf{a}}\\ {\dot{b}}_h\end{array}\right)=\left(\begin{array}{c}\frac{1}{2}\mathbf{q}\ast \left(\mathit{\omega }-{\mathbf{b}}_{\mathit{\omega }}\right)\\ \mathbf{g}+\frac{1}{s_{\mathbf{a}}}\mathbf{q}\ast \mathbf{a}\ast {\mathbf{q}}^{-1}\\ \mathbf{v}\\ 0\\ 0\\ 0\end{array}\right).$$

(2)

The measurements include the velocity and location from the GPS, altitude from a barometric sensor, and magnetic field from a magnetometer in the MEMS-AHRS as follows: $\mathbf{z}={({\mathbf{z}}_{\mathbf{v}},{\mathbf{z}}_{\mathbf{x}},z_h,z_{\mathbf{m}})}^T$. The measurements are related to the state as shown by the measurement model (3):

$$\mathbf{z}=\left(\begin{array}{c}{\mathbf{z}}_{\mathbf{v}}\\ {\mathbf{z}}_{\mathbf{x}}\\ z_h\\ z_{\mathbf{m}}\end{array}\right)=\left(\begin{array}{c}\mathbf{v}\\ \mathbf{x}\\ \mathbf{x}·{\mathbf{e}}_z+b_h\\ q^{-1}\ast \mathbf{m}\ast q\end{array}\right).$$

(3)

In the application of the right IEKF, the process and measurement model incorporates process noise $\mathbf{w}={\left({\mathbf{w}}_{\mathbf{q}},{\mathbf{w}}_{\mathbf{v}},{\mathbf{w}}_{\mathbf{x}},{\mathbf{w}}_{{\mathbf{b}}_{\mathit{\omega }}},{\mathbf{w}}_{s_{\mathbf{a}}},{\mathbf{w}}_{b_h}\right)}^T$ and measurement noise $\mathit{\nu }={({\mathit{\nu }}_{\mathbf{v}},{\mathit{\nu }}_{\mathbf{x}},{\mathit{\nu }}_h,{\mathit{\nu }}_{\mathbf{m}})}^T$ in models (2) and (3), resulting in Equations (4) and (5):

$$\dot{\mathit{\chi }}=\left(\begin{array}{c}\dot{\mathbf{q}}\\ \dot{\mathbf{v}}\\ \dot{\mathbf{x}}\\ {\dot{\mathbf{b}}}_{\mathit{\omega }}\\ {\dot{s}}_{\mathbf{a}}\\ {\dot{b}}_h\end{array}\right)=\left(\begin{array}{c}\frac{1}{2}\mathbf{q}\ast \left(\mathit{\omega }-{\mathbf{b}}_{\mathit{\omega }}\right)\\ \mathbf{g}+\frac{1}{s_{\mathbf{a}}}\mathbf{q}\ast \mathbf{a}\ast {\mathbf{q}}^{-1}\\ \mathbf{v}\\ 0\\ 0\\ 0\end{array}\right)+\left(\begin{array}{c}{\mathbf{M}}_{\mathbf{q}}{\mathbf{w}}_{\mathbf{q}}\ast \mathbf{q}\\ {\mathbf{M}}_{\mathbf{v}}{\mathbf{w}}_{\mathbf{v}}\\ {\mathbf{M}}_{\mathbf{x}}{\mathbf{w}}_{\mathbf{x}}\\ {\mathbf{q}}^{-1}\ast {\mathbf{M}}_{{\mathbf{b}}_{\mathit{\omega }}}{\mathbf{w}}_{{\mathbf{b}}_{\mathit{\omega }}}\ast \mathbf{q}\\ s_{\mathbf{a}}{\mathbf{M}}_{s_{\mathbf{a}}}{\mathbf{w}}_{s_{\mathbf{a}}}\\ {\mathbf{M}}_{b_h}{\mathbf{w}}_{b_h}\end{array}\right)$$

(4)

$$\mathbf{z}=\left(\begin{array}{c}{\mathbf{z}}_{\mathbf{v}}\\ {\mathbf{z}}_{\mathbf{x}}\\ z_h\\ z_{\mathbf{m}}\end{array}\right)=\left(\begin{array}{c}\mathbf{v}+{\mathbf{N}}_{\mathbf{v}}{\mathit{\nu }}_{\mathbf{v}}\\ \mathbf{x}+{\mathbf{N}}_{\mathbf{x}}{\nu }_{\mathbf{x}}\\ \mathbf{x}·{\mathbf{e}}_z+b_h+{\mathbf{N}}_h{\mathit{\nu }}_h\\ {\mathbf{q}}^{-1}\ast \left(\mathbf{m}+{\mathbf{N}}_{\mathbf{m}}{\mathit{\nu }}_{\mathbf{m}}\right)\ast \mathbf{q}\end{array}\right).$$

(5)

The inclusion of noise in the process and measurement model, as shown in Equations (4) and (5), transforms the problem for the application of the IEKF, and the IEKF estimates the state and error covariance $\mathbf{P}$ as shown in Equations (6) and (7), respectively:

$$\dot{\widehat{\mathit{\chi }}}=\left(\begin{array}{c}\dot{\widehat{\mathbf{q}}}\\ \dot{\widehat{\mathbf{v}}}\\ \dot{\widehat{\mathbf{x}}}\\ {\dot{\widehat{\mathbf{b}}}}_{\mathit{\omega }}\\ {\dot{\widehat{s}}}_{\mathbf{a}}\\ {\dot{\widehat{b}}}_h\end{array}\right)=\left(\begin{array}{c}\frac{1}{2}\widehat{\mathbf{q}}\ast \left(\mathit{\omega }-{\widehat{\mathbf{b}}}_{\mathit{\omega }}\right)\\ \mathbf{g}+\frac{1}{{\widehat{s}}_{\mathbf{a}}}\widehat{\mathbf{q}}\ast \mathbf{a}\ast {\widehat{\mathbf{q}}}^{-1}\\ \widehat{\mathbf{v}}\\ 0\\ 0\\ 0\end{array}\right)+\left(\begin{array}{c}{\mathbf{K}}_{\mathbf{q}}\mathbf{E}\ast \widehat{\mathbf{q}}\\ {\mathbf{K}}_{\mathbf{v}}\mathbf{E}\\ {\mathbf{K}}_{\mathbf{x}}\mathbf{E}\\ {\widehat{\mathbf{q}}}^{-1}\ast {\mathbf{K}}_{{\mathbf{b}}_{\mathit{\omega }}}\mathbf{E}\ast \widehat{\mathbf{q}}\\ {\widehat{s}}_{\mathbf{a}}{\mathbf{K}}_{s_{\mathbf{a}}}\mathbf{E}\\ {\mathbf{K}}_{b_h}\mathbf{E}\end{array}\right),$$

(6)

$$\dot{\mathbf{P}}=\mathbf{A}\mathbf{P}+\mathbf{P}{\mathbf{A}}^T+\mathbf{M}{\mathbf{M}}^T-\mathbf{P}{\mathbf{C}}^T{\left(\mathbf{N}{\mathbf{N}}^T\right)}^{-1}\mathbf{C}\mathbf{P},$$

(7)

where process error covariance $\mathbf{M}$ and measurement error covariance $\mathbf{N}$ are given in Equation (8):

$$\begin{array}{c}\mathbf{M}=Diag({\mathbf{M}}_{\mathbf{q}},{\mathbf{M}}_{\mathbf{v}},{\mathbf{M}}_{\mathbf{x}},{\mathbf{M}}_{{\mathbf{b}}_{\mathit{\omega }}},{\mathbf{M}}_{s_{\mathbf{a}}},{\mathbf{M}}_{b_h}),\hfill \\ \mathbf{N}=Diag({\mathbf{N}}_{\mathbf{v}},{\mathbf{N}}_{\mathbf{x}},{\mathbf{N}}_h,{\mathbf{N}}_{\mathbf{m}}).\hfill \end{array}$$

(8)

In Equations (6) and (7) for the state $\mathit{\chi }$ and error covariance $\mathbf{P}$, the Kalman gain $\mathbf{K}$ and output error $\mathbf{E}$ are given by Equations (9) and (10), respectively:

$$\mathbf{K}=-{({\mathbf{K}}_{\mathbf{q}},{\mathbf{K}}_{\mathbf{v}},{\mathbf{K}}_{\mathbf{x}},{\mathbf{K}}_{{\mathbf{b}}_{\mathit{\omega }}},{\mathbf{K}}_{s_{\mathbf{a}}},{\mathbf{K}}_{b_h})}^T=\mathbf{P}{\mathbf{C}}^T{\left(\mathbf{N}{\mathbf{N}}^T\right)}^{-1},$$

(9)

$$\mathbf{E}=\left(\begin{array}{c}{\widehat{\mathbf{z}}}_{\mathbf{v}}-{\mathbf{z}}_{\mathbf{v}}\\ {\widehat{\mathbf{z}}}_{\mathbf{x}}-{\mathbf{z}}_{\mathbf{x}}\\ {\widehat{z}}_h-z_h\\ \mathbf{m}-\widehat{\mathbf{q}}\ast {\mathbf{z}}_{\mathbf{m}}\ast {\widehat{\mathbf{q}}}^{-1}\end{array}\right)=\left(\begin{array}{c}\widehat{\mathbf{v}}-\mathbf{v}-{\mathbf{N}}_{\mathbf{v}}{\mathit{\nu }}_{\mathbf{v}}\\ \widehat{\mathbf{x}}-\mathbf{x}-{\mathbf{N}}_{\mathbf{x}}{\mathit{\nu }}_{\mathbf{x}}\\ (\widehat{\mathbf{x}}-\mathbf{x})·{\mathbf{e}}_z+{\widehat{b}}_h-b_h-{\mathbf{N}}_h{\mathit{\nu }}_h\\ \mathbf{m}-\widehat{\mathbf{q}}\ast {\mathbf{q}}^{-1}\ast \left(\mathbf{m}+{\mathbf{N}}_{\mathbf{m}}{\mathit{\nu }}_{\mathbf{m}}\right)\ast \mathbf{q}\ast {\widehat{\mathbf{q}}}^{-1}\end{array}\right).$$

(10)

The calculation of Kalman gain $\mathbf{K}$ and state error covariance $\mathbf{P}$ requires linearized state transition coefficient $\mathbf{A}$ and measurement coefficient $\mathbf{C}$ as shown in Equations (7) and (9). Usual EKF takes the partial derivatives of process model and measurement model with respect to state as the matrices $\mathbf{A}$ and $\mathbf{C}$, respectively. On the contrary, IEKF finds the matrices $\mathbf{A}$ and $\mathbf{C}$ in a different way from the usual EKF because the IEKF takes state error differently from that of the usual EKF [8,16]. To make the observer invariant and adapt the problem for IEKF application, the state error $\mathit{\eta }={({\mathit{\eta }}_{\mathbf{q}},{\mathit{\eta }}_{\mathbf{v}},{\mathit{\eta }}_{\mathbf{x}},{\mathit{\eta }}_{{\mathbf{b}}_{\mathit{\omega }}},{\mathit{\eta }}_{s_{\mathbf{a}}},{\mathit{\eta }}_{b_h})}^T$ is taken as Equation (11) [2]:

$$\left(\begin{array}{c}{\mathit{\eta }}_{\mathbf{q}}\\ {\mathit{\eta }}_{\mathbf{v}}\\ {\mathit{\eta }}_{\mathbf{x}}\\ {\mathit{\eta }}_{{\mathbf{b}}_{\mathit{\omega }}}\\ {\mathit{\eta }}_{s_{\mathbf{a}}}\\ {\mathit{\eta }}_{b_h}\end{array}\right)=\left(\begin{array}{c}\widehat{\mathbf{q}}\ast {\mathbf{q}}^{-1}\\ \widehat{\mathbf{v}}-\mathbf{v}\\ \widehat{\mathbf{x}}-\mathbf{x}\\ \mathbf{q}\ast \left({\widehat{\mathbf{b}}}_{\mathit{\omega }}-{\mathbf{b}}_{\mathit{\omega }}\right)\ast {\mathbf{q}}^{-1}\\ \frac{{\widehat{s}}_{\mathbf{a}}}{s_{\mathbf{a}}}\\ {\widehat{b}}_h-b_h\end{array}\right).$$

(11)

Unlike the usual EKF where state error is taken as the algebraic difference between estimated state and true state, the state error in IEKF has a different form as shown at the first, fourth, and fifth components of the state error vector in Equation (11).

For usual EKF, the system equation for state error differential, which is derived from the state estimation equation for usual EKF and the usual process model, is shown as Equation (12) [8]:

$$\delta \dot{\mathit{\eta }}=(\mathbf{A}-\mathbf{K}\mathbf{C})\delta \mathit{\eta }-\mathbf{M}\mathbf{w}+\mathbf{K}\mathbf{N}\mathit{\nu }.$$

(12)

Likewise, for the IEKF, the system equation for state error differential $\dot{\mathit{\eta }}$ can be found by combining Equations (4), (6), (10), and (11). Linearizing the system equation for state error differential $\dot{\mathit{\eta }}$ of the IEKF and dropping all the quadratic terms for the noise and infinitesimal state error results in a linearized state error differential system, which is equivalent to Equation (12). Finding the correspondence between the linearized state error differential system for IEKF and Equation (12) for usual EKF yields the linearized process transition coefficient $\mathbf{A}$ and measurement coefficient $\mathbf{C}$ for IEKF as Equations (13) and (14), respectively [2]:

$$\mathbf{A}=\left(\begin{array}{cccccc}{\mathbf{0}}_{33}& {\mathbf{0}}_{33}& {\mathbf{0}}_{33}& -\frac{1}{2}{\mathbf{I}}_3& {\mathbf{0}}_{31}& {\mathbf{0}}_{31}\\ -2{\widehat{\mathbf{I}}}_{\mathbf{a}\times }& {\mathbf{0}}_{33}& {\mathbf{0}}_{33}& {\mathbf{0}}_{33}& -{\widehat{\mathbf{I}}}_{\mathbf{a}}& {\mathbf{0}}_{31}\\ {\mathbf{0}}_{33}& {\mathbf{I}}_3& {\mathbf{0}}_{33}& {\mathbf{0}}_{33}& {\mathbf{0}}_{31}& {\mathbf{0}}_{31}\\ {\mathbf{0}}_{33}& {\mathbf{0}}_{33}& {\mathbf{0}}_{33}& {\widehat{\mathbf{I}}}_{\mathit{\omega }\times }& {\mathbf{0}}_{31}& {\mathbf{0}}_{31}\\ {\mathbf{0}}_{13}& {\mathbf{0}}_{13}& {\mathbf{0}}_{13}& {\mathbf{0}}_{13}& {\mathbf{0}}_{11}& {\mathbf{0}}_{11}\\ {\mathbf{0}}_{13}& {\mathbf{0}}_{13}& {\mathbf{0}}_{13}& {\mathbf{0}}_{13}& {\mathbf{0}}_{11}& {\mathbf{0}}_{11}\end{array}\right),$$

(13)

$$\mathbf{C}=\left(\begin{array}{cccccc}{\mathbf{0}}_{33}& {\mathbf{I}}_3& {\mathbf{0}}_{33}& {\mathbf{0}}_{33}& {\mathbf{0}}_{31}& {\mathbf{0}}_{31}\\ {\mathbf{0}}_{33}& {\mathbf{0}}_{33}& {\mathbf{I}}_3& {\mathbf{0}}_{33}& {\mathbf{0}}_{31}& {\mathbf{0}}_{31}\\ {\mathbf{0}}_{13}& {\mathbf{0}}_{13}& {\mathbf{e}}_z^T& {\mathbf{0}}_{13}& {\mathbf{0}}_{11}& -{\mathbf{I}}_1\\ 2{\mathbf{m}}_{\times }& {\mathbf{0}}_{33}& {\mathbf{0}}_{33}& {\mathbf{0}}_{33}& {\mathbf{0}}_{31}& {\mathbf{0}}_{31}\end{array}\right).$$

(14)

In $\mathbf{A}$ and $\mathbf{C}$, the invariant quantities ${\widehat{\mathbf{I}}}_{\mathit{\omega }}$ and ${\widehat{\mathbf{I}}}_{\mathit{a}}$ are given in the following Equation (15):

$$\begin{array}{c}{\widehat{\mathbf{I}}}_{\mathit{\omega }}=\widehat{\mathbf{q}}\ast \left(\mathit{\omega }-{\widehat{\mathbf{b}}}_{\mathit{\omega }}\right)\ast {\widehat{\mathbf{q}}}^{-1},\hfill \\ {\widehat{\mathbf{I}}}_{\mathbf{a}}=\frac{1}{{\widehat{s}}_{\mathbf{a}}}\widehat{\mathbf{q}}\ast \mathbf{a}\ast {\widehat{\mathbf{q}}}^{-1}.\hfill \end{array}$$

(15)

${\mathbf{I}}_{\times }$ is defined such that ${\mathbf{I}}_{\times }\mathbf{u}=\mathbf{I}\times \mathbf{u}$ for all $\mathbf{u}\in {\mathbf{R}}^3$.

It should be noticed that matrices $\mathbf{A}$ and $\mathbf{C}$ are constant if invariant quantities ${\widehat{\mathbf{I}}}_{\mathit{\omega }}$ and ${\widehat{\mathbf{I}}}_{\mathit{a}}$ are constant. ${\widehat{\mathbf{I}}}_{\mathit{\omega }}$ and ${\widehat{\mathbf{I}}}_{\mathit{a}}$ are constant if the UAV flies at a constant angular rate and there is constant acceleration in the NED coordinate frame. If these conditions are met, the IEKF becomes a linear Kalman filter, which guarantees convergence. This is one of the most distinctive features of the IEKF compared with the usual EKF. In addition, the differential equation for the quaternion estimation derived in Equation (6) intrinsically keeps the estimated quaternion normalized at all times. These advantageous features apply to all IEKFs, including the IEKF-av, which will be described in Section 3.2.

In case of usual EKF, the matrices $\mathbf{A}$ and $\mathbf{C}$ are derived from partial derivatives of process model and measurement model. Since the process model and measurement model are a nonlinear combination of attitude, angular rate, acceleration, bias in angular rate, and acceleration scale factor, the partial derivatives depend on those variables in a nonlinear manner, and vary with a change of those variables, which is not desirable for convergence. If all the variables are constant, the matrices $\mathbf{A}$ and $\mathbf{C}$ will be constant and the EKF also will work like a linear KF, assuring convergence. However, even though the UAV flies at a constant angular rate and constant acceleration in the NED coordinate frame, the matrices $\mathbf{A}$ and $\mathbf{C}$ are not constant, that is, nonlinear dynamics do not become linear, therefore the properties applicable to linear KF do not apply. On the contrary, in IEKF, the matrices $\mathbf{A}$ and $\mathbf{C}$ are constant in such a systematic way that $\mathbf{A}$ and $\mathbf{C}$ become constant if the UAV flies at a constant angular rate and constant acceleration in the NED coordinate frame as explained above. This advantage of IEKF over usual EKF is due to the use of invariance property in derivation of the IEKF [8,16].

Putting the above descriptions together, IEKF formulates the process model and measurement model to be invariant as Equations (4) and (5), thus making it possible to design the observer [estimator] to be invariant as Equation (6). Due to the invariance property of the observer (6), the sufficient condition for constant $\mathbf{A}$ and $\mathbf{C}$ becomes that invariant quantities ${\widehat{\mathbf{I}}}_{\mathit{\omega }}$ and ${\widehat{\mathbf{I}}}_{\mathit{a}}$ be constant. This sufficient condition for IEKF is relaxed compared with that for the usual EKF, for which attitude, angular rate, acceleration, bias in angular rate, and acceleration scale factor are required to be constant to keep $\mathbf{A}$ and $\mathbf{C}$ constant—thus formulating the process and measurement model in an invariant way and designing the observer to be invariant result in improved convergence property of the IEKF over the usual EKF [8].

In addition to the advantage of convergence property, the quaternion estimation $\widehat{\mathbf{q}}$ of the IEKF intrinsically keeps the unit norm constraint onto the quaternion. The magnitude $\parallel \widehat{\mathbf{q}}\parallel$ of the estimated quaternion is kept as a unit since the quaternion estimate obeys the differential equation presented at the first component of the observation Equation (6) [8], which is rephrased as Equation (16):

$$\dot{\widehat{\mathbf{q}}}=\frac{1}{2}\widehat{\mathbf{q}}\ast \left(\mathit{\omega }-{\widehat{\mathbf{b}}}_{\mathit{\omega }}\right)+{\mathbf{K}}_{\mathbf{q}}\mathbf{E}\ast \widehat{\mathbf{q}}.$$

(16)

The second term ${\mathbf{K}}_{\mathbf{q}}\mathbf{E}\ast \widehat{\mathbf{q}}$ of Equation (16), which is multiplication of quaternion $\widehat{\mathbf{q}}$ with the vector ${\mathbf{K}}_{\mathbf{q}}\mathbf{E}$, plays the role of preserving the magnitude of the quaternion constant [8]. In usual EKF, the corresponding term will be replaced by the multiplication of Kalman gain with measurement innovation. Therefore, the magnitude of the estimated quaternion deviates from the unit, thus invalid adjustment of the quaternion to rescale the magnitude to be a unit is inevitable. This deteriorates estimation performance of the usual EKF compared with the IEKF.

3.2. IEKF-av: Right IEKF for Estimation of Attitude and Velocity

This section describes the right IEKF used for estimating the attitude and velocity [8]. The state vector $\mathit{\chi }$ to be estimated consists of four variables: the attitude, velocity, bias in the angular rate measurement, and scale factor of the acceleration measurement (17):

$$\mathit{\chi }={(\mathbf{q},\mathbf{v},{\mathbf{b}}_{\mathit{\omega }},s_{\mathbf{a}})}^T.$$

(17)

The process model for the state transition is the same as Equation (2), except that two equations for the location $\mathbf{x}$ and bias in the altitude measurement $b_h$ are deleted, resulting in Equation (18):

$$\dot{\mathit{\chi }}=\left(\begin{array}{c}\dot{\mathbf{q}}\\ \dot{\mathbf{v}}\\ {\dot{\mathbf{b}}}_{\mathit{\omega }}\\ {\dot{s}}_{\mathbf{a}}\end{array}\right)=\left(\begin{array}{c}\frac{1}{2}\mathbf{q}\ast \left(\mathit{\omega }-{\mathbf{b}}_{\mathit{\omega }}\right)\\ \mathbf{g}+\frac{1}{s_{\mathbf{a}}}\mathbf{q}\ast \mathbf{a}\ast {\mathbf{q}}^{-1}\\ 0\\ 0\end{array}\right).$$

(18)

The measurements include the velocity from the GPS and the magnetic field from the magnetometer in the MEMS-AHRS as follows: $\mathbf{z}={({\mathbf{z}}_{\mathbf{v}},z_{\mathbf{m}})}^T$. Therefore, the measurement model is derived from that for the IEKF-lav (Equation (3)) by deleting the equations corresponding to the position and altitude measurements, resulting in Equation (19):

$$\mathbf{z}=\left(\begin{array}{c}{\mathbf{z}}_{\mathbf{v}}\\ z_{\mathbf{m}}\end{array}\right)=\left(\begin{array}{c}\mathbf{v}\\ q^{-1}\ast \mathbf{m}\ast q\end{array}\right)$$

(19)

The inclusion of process noise is the same as that for the IEKF-lav, except that the equations for the states of the position and bias in the altitude measurement are deleted from Equation (2). Likewise, the noise incorporated measurement model is just Equation (5), except that the equations for ${\mathbf{z}}_{\mathbf{x}}$ and $z_h$ are deleted. The process model and measurement model for the IEKF-av are Equations (20) and (21):

$$\dot{\mathit{\chi }}=\left(\begin{array}{c}\dot{\mathbf{q}}\\ \dot{\mathbf{v}}\\ {\dot{\mathbf{b}}}_{\mathit{\omega }}\\ {\dot{s}}_{\mathbf{a}}\end{array}\right)=\left(\begin{array}{c}\frac{1}{2}\mathbf{q}\ast \left(\mathit{\omega }-{\mathbf{b}}_{\mathit{\omega }}\right)\\ \mathbf{g}+\frac{1}{s_{\mathbf{a}}}\mathbf{q}\ast \mathbf{a}\ast {\mathbf{q}}^{-1}\\ 0\\ 0\end{array}\right)+\left(\begin{array}{c}{\mathbf{M}}_{\mathbf{q}}{\mathbf{w}}_{\mathbf{q}}\ast \mathbf{q}\\ {\mathbf{M}}_{\mathbf{v}}{\mathbf{w}}_{\mathbf{v}}\\ {\mathbf{q}}^{-1}\ast {\mathbf{M}}_{{\mathbf{b}}_{\mathit{\omega }}}{\mathbf{w}}_{{\mathbf{b}}_{\mathit{\omega }}}\ast \mathbf{q}\\ s_{\mathbf{a}}{\mathbf{M}}_{s_{\mathbf{a}}}{\mathbf{w}}_{s_{\mathbf{a}}}\end{array}\right),$$

(20)

$$\mathbf{z}=\left(\begin{array}{c}{\mathbf{z}}_{\mathbf{v}}\\ z_{\mathbf{m}}\end{array}\right)=\left(\begin{array}{c}\mathbf{v}+{\mathbf{N}}_{\mathbf{v}}{\mathit{\nu }}_{\mathbf{v}}\\ {\mathbf{q}}^{-1}\ast \left(\mathbf{m}+{\mathbf{N}}_{\mathbf{m}}{\mathit{\nu }}_{\mathbf{m}}\right)\ast \mathbf{q}\end{array}\right).$$

(21)

The IEKF estimator for the attitude and velocity becomes the observer of Equation (22), and the corresponding error covariance $\mathbf{P}$ is determined by Equation (23):

$$\dot{\widehat{\mathit{\chi }}}=\left(\begin{array}{c}\dot{\widehat{\mathit{q}}}\\ \dot{\widehat{\mathbf{v}}}\\ {\dot{\widehat{\mathbf{b}}}}_{\mathit{\omega }}\\ {\dot{\widehat{s}}}_{\mathbf{a}}\end{array}\right)=\left(\begin{array}{c}\frac{1}{2}\widehat{\mathbf{q}}\ast \left(\mathit{\omega }-{\widehat{\mathbf{b}}}_{\mathit{\omega }}\right)\\ \mathbf{g}+\frac{1}{{\widehat{s}}_{\mathbf{a}}}\widehat{\mathbf{q}}\ast \mathbf{a}\ast {\widehat{\mathbf{q}}}^{-1}\\ 0\\ 0\end{array}\right)+\left(\begin{array}{c}{\mathbf{K}}_{\mathbf{q}}\mathbf{E}\ast \widehat{\mathbf{q}}\\ {\mathbf{K}}_{\mathbf{v}}\mathbf{E}\\ {\widehat{\mathbf{q}}}^{-1}\ast {\mathbf{K}}_{{\mathbf{b}}_{\mathit{\omega }}}\mathbf{E}\ast \widehat{\mathbf{q}}\\ {\widehat{s}}_{\mathbf{a}}{\mathbf{K}}_{s_{\mathbf{a}}}\mathbf{E}\end{array}\right),$$

(22)

$$\dot{\mathbf{P}}=\mathbf{A}\mathbf{P}+\mathbf{P}{\mathbf{A}}^T+\mathbf{M}{\mathbf{M}}^T-\mathbf{P}{\mathbf{C}}^T{\left(\mathbf{N}{\mathbf{N}}^T\right)}^{-1}\mathbf{C}\mathbf{P},$$

(23)

where process error covariance $\mathbf{M}$ and measurement error covariance $\mathbf{N}$ are given in Equation (24):

$$\begin{array}{c}\mathbf{M}=Diag({\mathbf{M}}_{\mathbf{q}},{\mathbf{M}}_{\mathbf{v}},{\mathbf{M}}_{{\mathbf{b}}_{\mathit{\omega }}},{\mathbf{M}}_{s_{\mathbf{a}}}),\hfill \\ \mathbf{N}=Diag({\mathbf{N}}_{\mathbf{v}},{\mathbf{N}}_{\mathbf{m}}).\hfill \end{array}$$

(24)

In Equations (22) and (23) for state $\mathit{\chi }$ and error covariance $\mathbf{P}$, the Kalman gain $\mathbf{K}$ and output error $\mathbf{E}$ are given by Equations (25) and (26), respectively:

$$\mathbf{K}=-{({\mathbf{K}}_{\mathbf{q}},{\mathbf{K}}_{\mathbf{v}},{\mathbf{K}}_{{\mathbf{b}}_{\mathit{\omega }}},{\mathbf{K}}_{s_{\mathbf{a}}})}^T=\mathbf{P}{\mathbf{C}}^T{\left(\mathbf{N}{\mathbf{N}}^T\right)}^{-1},$$

(25)

$$\mathbf{E}=\left(\begin{array}{c}{\widehat{\mathbf{z}}}_{\mathbf{v}}-{\mathbf{z}}_{\mathbf{v}}\\ \mathbf{m}-\widehat{\mathbf{q}}\ast {\mathbf{z}}_{\mathbf{m}}\ast {\widehat{\mathbf{q}}}^{-1}\end{array}\right)=\left(\begin{array}{c}\widehat{\mathbf{v}}-\mathbf{v}-{\mathbf{N}}_{\mathbf{v}}{\mathit{\nu }}_{\mathbf{v}}\\ \mathbf{m}-\widehat{\mathbf{q}}\ast {\mathbf{q}}^{-1}\ast \left(\mathbf{m}+{\mathbf{N}}_{\mathbf{m}}{\mathit{\nu }}_{\mathbf{m}}\right)\ast \mathbf{q}\ast {\widehat{\mathbf{q}}}^{-1}\end{array}\right).$$

(26)

To derive the linearized process model $\mathbf{A}$ and measurement model $\mathbf{C}$, which are required for calculating the Kalman gain $\mathbf{K}$ and state error covariance $\mathbf{P}$, the state error $\mathit{\eta }={({\mathit{\eta }}_{\mathbf{q}},{\mathit{\eta }}_{\mathbf{v}},{\mathit{\eta }}_{{\mathbf{b}}_{\mathit{\omega }}},{\mathit{\eta }}_{s_{\mathbf{a}}})}^T$ is taken as Equation (27):

$$\left(\begin{array}{c}{\mathit{\eta }}_{\mathbf{q}}\\ {\mathit{\eta }}_{\mathbf{v}}\\ {\mathit{\eta }}_{{\mathbf{b}}_{\mathit{\omega }}}\\ {\mathit{\eta }}_{s_{\mathbf{a}}}\end{array}\right)=\left(\begin{array}{c}\widehat{\mathbf{q}}\ast {\mathbf{q}}^{-1}\\ \widehat{\mathbf{v}}-\mathbf{v}\\ \mathbf{q}\ast \left({\widehat{\mathbf{b}}}_{\mathit{\omega }}-{\mathbf{b}}_{\mathit{\omega }}\right)\ast {\mathbf{q}}^{-1}\\ \frac{{\widehat{s}}_{\mathbf{a}}}{s_{\mathbf{a}}}\end{array}\right).$$

(27)

Linearizing the state error differential system and fitting the linearized model to Equation (12) yields matrices $\mathbf{A}$ and $\mathbf{C}$, as shown in Equations (28) and (29) [8], respectively:

$$\mathbf{A}=\left(\begin{array}{cccc}{\mathbf{0}}_{33}& {\mathbf{0}}_{33}& -\frac{1}{2}{\mathbf{I}}_3& {\mathbf{0}}_{31}\\ -2{\widehat{\mathbf{I}}}_{\mathbf{a}\times }& {\mathbf{0}}_{33}& {\mathbf{0}}_{33}& -{\widehat{\mathbf{I}}}_{\mathbf{a}}\\ {\mathbf{0}}_{33}& {\mathbf{0}}_{33}& {\widehat{\mathbf{I}}}_{\mathit{\omega }\times }& {\mathbf{0}}_{31}\\ {\mathbf{0}}_{13}& {\mathbf{0}}_{13}& {\mathbf{0}}_{13}& {\mathbf{0}}_{11}\end{array}\right),$$

(28)

$$\mathbf{C}=\left(\begin{array}{cccc}{\mathbf{0}}_{33}& {\mathbf{I}}_3& {\mathbf{0}}_{33}& {\mathbf{0}}_{31}\\ 2{\mathbf{m}}_{\times }& {\mathbf{0}}_{33}& {\mathbf{0}}_{33}& {\mathbf{0}}_{31}\end{array}\right).$$

(29)

3.3. ecl-EKF for Estimation of Attitude, Velocity, and Location

An EKF for estimating the attitude, velocity, and location, called the ecl-EKF, is used for comparison purposes [23]. The ecl-EKF is an EKF-based navigation algorithm that was developed by the PX4 project [22]. The ecl-EKF sets a state vector encompassing the following 24 state variables: the attitude by quaternion, velocity, position, IMU delta angle bias, IMU delta velocity bias, earth magnetic field, magnetometer bias, and horizontal wind velocity. It uses measurements of the angular rate and acceleration by the IMU, along with the GPS position, GPS velocity, pressure altitude, and geomagnetic field.

The ecl-EKF uses an output predictor and data buffer to fuse data from sensors with different time delays and data rates. The EKF interacts with the strap down inertial navigation unit, data buffer, and output predictor. The ecl-EKF is capable of fusing a large range of different sensor types. It also detects outliers in sensor measurements. The ecl-EKF code can be found at GitHub [24]. For the implementation and details of the ecl-EKF and PX4, please refer to the documents on PX4 at the site [25].

4. Implementation for Navigation of UAVs

The IEKF methods and ecl-EKF were implemented for the navigation of a four-rotary wing UAV, otherwise called a quadrotor, and a fixed wing UAV. The fixed wing UAV was used to test the IEKF-lav and ecl-EKF, and the quadrotor was used to test the IEKF-av and ecl-EKF. Figure 1 shows the quadrotor and fixed wing UAV used for the flight test. Each UAV had a global navigation satellite system (GNSS), an AHRS, and a barometric altimeter.

Table 1. Quadrotor used for navigation test.

Parameter	Specification
Model	S500 Glass Fiber Quadrotor Frame 480 mm
Distance between motor center	480 mm
Weight, frame only	405 g
Height, frame only	170 mm
Flight controller	Pixhawk v2

and

Table 2. Fixed wing unmanned aerial vehicles (UAV) used for navigation test.

Parameter	Specification
Model	ALBATROSS *
Wingspan	1800 mm
Weight (camera and battery included)	4.0 kg
Flight controller	Pixhawk V2

* This is different from the Albatross produced by the company Applied Aeronautics.

explain the UAVs used for the tests, and

Table 3. Global navigation satellite system (GNSS) used for the navigation test.

Parameter	Specification
Manufacturer	u-blox
Model	NEO-M8P
GNSS	GPS, GLONASS, BeiDou
Grade	Professional
Accuracy of time pulse signal	RMS 30 ns; 99% 60 ns
Velocity accuracy	0.05 m/s
Max navigation update rate	Raw 10 Hz
Horizontal position accuracy *	Standalone 2.5 m CEP; RTK 0.025 m + 1 ppm CEP

* Standalone mode is used in the test.

lists the performance of the GNSS used in the experiments.

The two UAVs use the same flight control unit of Pixhawk v2 (3DR, Berkeley, CA, US) [26]. Pixhawk v2 has an internal accelerometer, gyroscope, and barometer, in addition to a CPU and external magnetometer. The components are shown in

Table 4. Composition of flight control unit, Pixhawk v2.

Component	Model	Maker
CPU	STM32F427; flash 2MiB, RAM 256KiB	PX4 Team
Accelerometer, gyroscope	MPU9250	InvenSense Inc.
Barometer	MS5611	TE Connectivity company
External Magnetometer	HMC5983	Honeywell

. The gyroscope can measure up to 2000 deg/s of angular rate in three axes with maximum output data rate 8000 Hz, root mean square (RMS) noise 0.1 ${}^{\circ }/\mathrm{s}$, nonlinearity $\pm 0.1\%$, and noise spectral density 0.01 ${}^{\circ }/\mathrm{s}/\sqrt{\mathrm{Hz}}$. The accelerometer measures three axes’ acceleration at the maximum output data rate of 4000 Hz, with full scale range of ±16 g , nonlinearity $\pm 0.5\%$, RMS noise 8 mg , and noise power spectral density $300\mathsf{\mu }\mathrm{g}/\sqrt{\mathrm{Hz}}$. The barometer measures atmospheric pressure, which is transformed to height. Its measurement range is 10 to 1200 mbar, with accuracy of $\pm 1.5$ mbar at 25 ${}^{\circ }\mathrm{C}$ and 750 mbar atmosphere. Its error band is $\pm 2.5$ mbar at −20 ${}^{\circ }\mathrm{C}$ to +85 ${}^{\circ }\mathrm{C}$ temperature and a 450 to 1100 mbar environment. The magnetometer measures magnetic field strength in three axes, with the measurement range of $\pm 8$ Gauss, linearity $\pm 0.1\%$ of full scale at ±2.0 Gauss input range, hysteresis of $\pm 25$ ppm at $\pm 2.0$ Gauss input range, at the maximum output rate of 220 Hz.

The flight times were 700 s and 1800 s for the fixed wing UAV and quadrotor, respectively. The trajectories of the flights will be shown in the next sections, along with the estimation results (Figure 2).

The UAVs flew outdoors in open air, so that there was no interference to GNSS reception. The UAVs were remotely controlled using a dedicated joystick. The estimated states are not used for control of the UAVs. All the flight data including the measured sensor data are stored on board the flight control unit. Using the stored measurement data, the methods are applied offline for comparison. The methods do not run in real-time and are implemented using Matlab version R2018a (MathWorks, Natick, MA, US) on Windows 10 Pro desktop computer with CPU Intel(R) Core(TM) (Intel, Santa Clara, CA, US) i7-7700k CPU @4.20 GHz 4.20 GHz. For the IEKF-lav, each iteration takes a computation time of 0.084 ms. The computation time is less than and negligible compared to the IMU measurement update rate of 0.02 s within which the IEKF should iterate for possible real-time application.

In the experiments, measurement update rate of each sensor is as follows: data from AHRS-acceleration, angular rate, and magnetic field are measured at every 0.02 s, while GNSS velocity and position are sampled at every 0.2 s, and barometric altitude at every 0.1 s. IEKF is implemented to run at the rate of 0.02 s in synchronization with the time of AHRS measurement. For the other measurements, such as the position and velocity from GNSS and barometric altitude which are sensed at different rates from the AHRS measurement rate, the previous measurement data are used when there were no available measurement data at every iteration of the IEKF. One of the other possible methods is that whenever each measurement is available, a correction step per the available measurement is executed, which is the method used for ecl-EKF.

Application of IEKF and ecl-EKF requires tuning of parameter values. The process error covariance $\mathbf{M}$ and measurement error covariance $\mathbf{N}$ in Equations (8) and (24) affect the performance of the IEKF. They should be adjusted for each application depending on the sensor performance and system characteristics determining the process model Equations (4) and (18) and measurement model Equations (5) and (19). In our formulation, since the process model and measurement model are involved with the sensor measurements, they are mostly dependent on the sensor measurement performance. Since the IEKF-lav and IEKF-av use the same sensors and control unit, the parameter values for $\mathbf{M}$ and $\mathbf{N}$ are set to be the same. Considering the degree of uncertainty of each sensor measurement, and to result in the best performance in the sense that there is lower measurement innovation and faster convergence of the internal variables, the error covariance are set to Equation (30), through trial and error, to the best of the authors’ effort:

$$\begin{array}{c}{\mathbf{M}}_{\mathbf{q}}={\mathbf{M}}_{\mathbf{v}}={\mathbf{M}}_{\mathbf{x}}={\mathbf{M}}_{{\mathbf{b}}_{\mathit{\omega }}}=Diag(0.1,0.1,0.1),{\mathbf{M}}_{s_{\mathbf{a}}}={\mathbf{M}}_{b_h}=0.1,\hfill \\ {\mathbf{N}}_{\mathbf{v}}=Diag(0.5,0.5,0.5),{\mathbf{N}}_{\mathbf{x}}=Diag(0.1,0.1,0.1),{\mathbf{N}}_h=0.1,{\mathbf{N}}_{\mathbf{m}}=Diag(0.1,0.1,0.1).\hfill \end{array}$$

(30)

5. Discussion of Implementation Results

Two major comparisons of the results were made: (1) between the IEKF-lav and ecl-EKF for estimates of the location, attitude, and velocity, and (2) between the IEKF-av and ecl-EKF for the estimations of the attitude and velocity. The estimated state variables, Kalman gain, state error covariance, and measurement innovation were compared where appropriate.

The first comparison highlighted the distinctive properties of the IEKF relative to the usual EKF. The second comparison underscored the performance of the IEKF without location and altitude measurements compared with the EKF, for which location and altitude measurements were used. It showed that, even though there were no location and altitude measurements, the IEKF-av produced estimates of the velocity and attitude that were comparable to those of the ecl-EKF.

5.1. Comparison between IEKF-lav and ecl-EKF

Figure 2 compares the position estimates of the IEKF-lav and ecl-EKF for the flight of the fixed wing UAV. The IEKF-lav and ecl-EKF yielded similar position estimations on the $xy$ plane, while the $xz$ plane position estimates showed a difference. Although there was no definite evidence for which estimate was more accurate between the z directional location estimations of the IEKF-lav and ecl-EKF, the difference between the elevation estimates could be explained by the estimation of bias in the barometric altitude measurement. The IEKF-lav estimated the bias in the altitude measurement by the barometric altimeter, while the ecl-EKF does not. Figure 3 shows that the IEKF-lav estimated the bias in the altitude measurement to be approximately 15–20 m at 400–1600 s after the flight began. It should be noted that the difference between the altitude estimates of the IEKF-lav and ecl-EKF, as shown in Figure 2, corresponds approximately to the estimated bias in the barometric measurement of the IEKF-lav.

The difference between the altitudes estimated by IEKF-lav and the ecl-EKF corresponds to the bias estimated by the IEKF-lav, since the ecl-EKF neither estimates nor calibrates the bias while IEKF-lav does. Barometric pressure is transformed to the altitude. To get a more accurate altitude from the measured barometric pressure, it is required to calibrate the transformed altitude taking the local atmospheric pressure into account. However, the experiment uses the transformed altitude without the calibration. Therefore, estimating the error and correcting the altitude is desirable since the transformed altitude differs from the true altitude. The IEKF regards the error as the bias in altitude measurement. The bias is estimated and corrected using the altitude which is represented in state, as indicated by the third row of the measurement model Equation (3). It is shown as Equation (31):

$$z_h=\mathbf{x}·{\mathbf{e}}_z+b_h.$$

(31)

In Equation (31), the term $\mathbf{x}·{\mathbf{e}}_z$ represents the altitude element of the location state $\mathbf{x}$. The bias $b_h$ is the difference between the altitude $z_h$ transformed from barometric pressure measurement and the altitude $\mathbf{x}·{\mathbf{e}}_z$. If $\mathbf{x}$ is estimated as $\widehat{\mathbf{x}}$, the bias is the difference between the estimated altitude and the measured altitude. Since ecl-EKF does not take the bias into account, the altitude estimated by ecl-EKF differs from the altitude estimated by IEKF-lav, by the amount corresponding to the estimated bias.

Figure 4 and Figure 5 show the estimates of the velocity and attitude. The estimates by the IEKF-lav and ecl-EKF are similar, and there is no clear indication of which estimate is better than the other, as is the case for the comparison of the position estimations shown in Figure 2.

Figure 6 depicts the estimated bias in the angular rate measurement by the gyroscope. Unlike the estimation results for the velocity, attitude, and position in the $xy$ plane, the estimations of the bias in angular rate show a difference. The bias estimated by the IEKF-lav varies less over time than does the bias by the ecl-EKF. This is one of the distinctive features of the IEKF compared with the EKF. The estimation results and internal parameters of the IEKF are more convergent than those of the EKF.

Figure 7 compares the change in the Kalman gain by the IEKF-lav and ecl-EKF. The Kalman gain by the ecl-EKF changes periodically, as shown in Figure 7b. A comparison of Figure 5 and Figure 7b shows that the change in the Kalman gain occurs in the ecl-EKF case when the attitude changes during the flight. In contrast, the Kalman gain of the IEKF-lav does not change with a change in the heading, even though it also suffers from high-frequency jitter just like the ecl-EKF. This is the most salient feature of the IEKF compared with the EKF. Matrices $\mathbf{A}$ and $\mathbf{C}$ expressed by Equations (13), (14), (28), and (29) depend on invariant quantities ${\widehat{\mathbf{I}}}_{\mathit{\omega }}$, ${\widehat{\mathbf{I}}}_{\mathit{a}}$, and geomagnetic field $\mathbf{m}$ appropriate for a given local space. If the invariant quantities ${\widehat{\mathbf{I}}}_{\mathit{\omega }}$, ${\widehat{\mathbf{I}}}_{\mathit{a}}$, and geomagnetic field $\mathbf{m}$ remain constant, matrices $\mathbf{A}$ and $\mathbf{C}$ are also constant. Thus, the resulting Kalman gain $\mathbf{K}$ and state error covariance $\mathbf{P}$ converge to constants. As a whole, although it is not clearly verifiable that the estimated state variables for the IEKF-lav are better than those for the ecl-EKF, the analysis of the internal parameters such as the Kalman gain shows that the convergence property of the IEKF-lav outperforms that of the ecl-EKF.

Table 5. Comparison of Kalman gain for estimation of location, attitude, and velocity in fixed wing flight test.

Kalman Gain Element	IEKF-lav			ecl-EKF
	${\mathit{\mu }}_{\mathit{K}}$	${\mathit{\sigma }}_{\mathit{K}}$	$\frac{{\mathit{\sigma }}_{\mathit{K}}}{|{\mathit{\mu }}_{\mathit{K}}|}$	${\mathit{\mu }}_{\mathit{K}}$	${\mathit{\sigma }}_{\mathit{K}}$	$\frac{{\mathit{\sigma }}_{\mathit{K}}}{|{\mathit{\mu }}_{\mathit{K}}|}$
$K_{q_y/z_{m_x}}$	0.6665	0.0970	0.1455	$-5.4978\times {10}^{-4}$	0.0010	1.8199
$K_{q_y/z_{m_y}}$	0.0100	0.0974	9.7085	$-3.4738\times {10}^{-4}$	0.0011	3.2052
$K_{q_y/z_{m_z}}$	−0.3907	0.0562	0.1438	$7.2672\times {10}^{-4}$	0.0010	1.4262
$K_{v_x/z_{m_x}}$	−3.8793	0.4596	0.1185	−0.0019	0.0311	16.5717
$K_{v_x/z_{m_y}}$	−0.3273	0.6017	1.8383	−0.0092	0.0331	3.6085
$K_{v_x/z_{m_z}}$	2.3121	0.2638	0.1141	$-8.3181\times {10}^{-4}$	0.0117	14.0127
$K_{y/z_{m_x}}$	0.0026	$6.1468\times {10}^{-4}$	0.2370	$-8.0811\times {10}^{-4}$	0.0204	25.2135
$K_{y/z_{m_y}}$	$-1.0957\times {10}^{-4}$	$5.6941\times {10}^{-5}$	5.1970	−0.0079	0.0243	3.0778
$K_{y/z_{m_z}}$	−0.0015	$3.919\times {10}^{-5}$	0.2614	$-3.4259\times {10}^{-4}$	0.0075	22.0134

MI: Measurement innovation; ${\mathit{\mu }}_K$: mean of Kalman gain; ${\mathit{\sigma }}_K$: standard deviation of Kalman gain; $\frac{{\mathit{\sigma }}_K}{|{\mathit{\mu }}_K|}$: standard deviation to mean ratio (SM ratio).

compares the convergence property of the Kalman gain numerically. The table lists the values for the mean, standard deviation, and ratio between the standard deviation and mean of the Kalman gain for the IEKF-lav and ecl-EKF.

To clarify the elements of Kalman gain matrix to be compared, each element of the Kalman gain (9) is represented as $K_{\mathsf{x}/\mathsf{z}}$, as shown in Equation (32):

$$\begin{array}{cc}\multicolumn{2}{c}{\mathbf{K}=\left(\begin{array}{cccccc}{K}_{q_0/z_{v_x}}& K_{q_0/z_{v_y}}& K_{q_0/z_{v_z}}& K_{q_0/z_x}& \cdots & K_{q_0/z_{m_z}}\\ K_{q_x/z_{v_x}}& K_{q_x/z_{v_y}}& K_{q_x/z_{v_z}}& K_{q_x/z_x}& \cdots & K_{q_x/z_{m_z}}\\ K_{q_y/z_{v_x}}& ·& ·& ·& \cdots & K_{q_y/z_{m_z}}\\ ⋮& ⋮\\ K_{b_h/z_{v_x}}& K_{b_h/z_{v_y}}& K_{b_h/z_{v_z}}& K_{b_h/z_x}& \cdots & K_{b_h/z_{m_z}}\end{array}\right)={\left(\begin{array}{c}{K}_{\mathsf{x}/\mathsf{z}}\end{array}\right)}_{\mathsf{x}\in \mathbf{X},\mathsf{z}\in \mathbf{Z}},}\\ \begin{array}{cc}\mathrm{where}\hfill & \mathbf{X}=\{q_0,q_x,q_y,q_z,v_x,v_y,v_z,x,y,z,b_{w_x},b_{w_y},b_{w_z},s_{\mathbf{a}},b_h\},\hfill \\ & \mathbf{Z}=\{z_{v_x},z_{v_y},z_{v_z},z_x,z_y,z_z,z_h,z_{m_x},z_{m_y},z_{m_z}\}.\hfill \end{array}\end{array}$$

(32)

Here, the Kalman gain $K_{\mathsf{x}/\mathsf{z}}$ refers to the gain for the correction of the state $\mathsf{x}$ with respect to the innovation in the measurement $\mathsf{z}$. Table 5 lists the statistics for nine elements of the Kalman gain matrix. It shows that, out of the nine standard deviation to mean ratio (SM ratio) values, seven of them for the IEKF-lav are less than those for the ecl-EKF. Only for the two Kalman gains $K_{q_y/z_{m_y}}$ and $K_{y/z_{m_y}}$, the SM ratios for the ecl-EKF are less than those for the IEKF-lav. Thus, it can be concluded that the Kalman gains for the IEKF-lav are more convergent than those for the ecl-EKF.

There was also a crucial covariance difference between the IEKF-lav and ecl-EKF. Figure 8 compares the covariance for the state of the quaternion variables ${\left(q_x\left(t\right),q_y\left(t\right),q_z\left(t\right)\right)}^T$. The variation of the covariance with time for the IEKF-lav is less than that for the ecl-EKF.

Table 6. Comparison of error covariance for estimation of location, attitude, and velocity in fixed wing flight test.

State	Variable	IEKF-lav			ecl-EKF
		${\mathit{\mu }}_{\mathbf{C}}$	${\mathit{\sigma }}_{\mathbf{C}}$	$\frac{{\mathit{\sigma }}_{\mathbf{C}}}{{\mathit{\mu }}_{\mathbf{C}}}$	${\mathit{\mu }}_{\mathbf{C}}$	${\mathit{\sigma }}_{\mathbf{C}}$	$\frac{{\mathit{\sigma }}_{\mathbf{C}}}{{\mathit{\mu }}_{\mathbf{C}}}$
Quaternion	$q_x$	0.0094	0.0024	0.2523	$1.1568\times {10}^{-5}$	$4.9706\times {10}^{-6}$	0.4297
	$q_y$	0.0086	0.0012	0.1349	$1.2188\times {10}^{-5}$	$5.3028\times {10}^{-6}$	0.4351
	$q_z$	0.0369	0.0076	0.2073	$2.9485\times {10}^{-5}$	$2.5857\times {10}^{-5}$	0.8769
Velocity	$v_x$	0.6990	0.0534	0.0764	0.0161	0.0047	0.2893
	$v_y$	0.7653	0.0490	0.0640	0.0162	0.0044	0.2750
	$v_z$	0.5161	0.0568	0.1100	0.0093	0.0024	0.2611
Position	x	0.0100	$6.5039\times {10}^{-4}$	0.0649	0.0563	0.0211	0.3752
	y	0.0100	$6.5039\times {10}^{-4}$	0.0649	0.0564	0.0212	0.3757
	z	0.0090	$5.6567\times {10}^{-4}$	0.0629	0.0673	0.0098	0.1457
Bias in	${\omega }_x$	0.0271	0.0023	0.0841	$1.7199\times {10}^{-10}$	$2.8369\times {10}^{-11}$	0.1649
angular	${\omega }_y$	0.0268	0.0021	0.0798	$1.7036\times {10}^{-10}$	$2.4400\times {10}^{-11}$	0.1432
rate	${\omega }_z$	0.0357	0.0031	0.0859	$2.6572\times {10}^{-10}$	$3.4483\times {10}^{-11}$	0.1298

${\mathit{\mu }}_{\mathbf{C}}$: mean of covariance; ${\mathit{\sigma }}_{\mathbf{C}}$: standard deviation of covariance; $\frac{{\mathit{\sigma }}_{\mathbf{C}}}{{\mathit{\mu }}_{\mathbf{C}}}$: standard deviation to mean ratio (SM ratio).

lists the results of a statistical analysis of the variation with time based on the index of the SM ratio, as was used for the Kalman gain comparison. Both Figure 8 and Table 6 indicate that the convergence of the covariance for the IEKF-lav outperforms that for the ecl-EKF. This was due to the same justification as in the case for the convergence of the Kalman gain.

Figure 9 shows the measurement innovation for the altitude. The mean and standard deviation of the measurement innovation for the IEKF-lav are $-0.0012$ m and $0.3147$ m, while those for the ecl-EKF are $-0.0333$ m and $0.5147$ m in Figure 9, respectively. As can be expected from the position estimation result in the z- direction (Figure 2b), the measurement innovation for the ecl-EKF is a little larger than that for the IEKF-lav.

For the IEKF-lav, each iteration takes the computation time of 0.084 ms. The computation time is less than and negligible compared to the IMU measurement update rate of 0.02 s within which the IEKF should iterate for possible real-time application. One iteration of ecl-EKF takes 4.3 ms which is also short enough for real-time application compared to the IMU measurement rate. The computation time for ecl-EKF is 51 times longer than that for IEKF-lav. However, for the sake of fair comparison, it should be noticed that the ecl-EKF method includes calibration of IMU data before application of EKF and it estimates state variables such as bias of delta angle, bias of delta velocity, earth magnetic field, and horizontal wind velocity which are not estimated in IEKF-lav. Therefore, it can be concluded that the computation time given above suggests that IEKF-lav is computationally better than or at least comparable with the usual EKF, though the figures given above are not absolute ones applicable to general IEKF and usual EKF.

5.2. Comparison between IEKF-av and ecl-EKF

This section compares the IEKF-av and ecl-EKF based on the estimation results for the attitude and velocity during the quadrotor flight test. The IEKF-av does not use position and altitude measurements, while the ecl-EKF does use them. Although an analysis of the IEKF-lav is not intended in this section, the flight trajectory estimated by the IEKF-lav is shown in Figure 10 for the purpose of easily understanding the test conditions. The change in attitude can be determined from Figure 10.

Figure 11 shows the attitude estimations for the IEKF-av and ecl-EKF, and Figure 12 shows the velocity estimations. As suggested by the location, attitude, and velocity estimation results for the IEKF-lav and ecl-EKF, the IEKF-av and ecl-EKF have comparable attitude and velocity estimation results. Although it is not possible to say which is better, this verifies that the IEKF-av works well without location and altitude measurements.

The bias estimations for the angular rate measurements showed a distinctive difference, and Figure 13 compares the estimated bias values. While the bias in the angular rate measurement for the fixed wing UAV flight shows a difference in the average magnitude, as shown in Figure 6 of Section 5.1, the bias in the quadrotor flight test shows a difference in the degree of fluctuation. The bias for the ecl-EKF shows a larger fluctuation than that for the IEKF-av, as shown in Figure 13.

The Kalman gain and state error covariance most critically discriminate the IEKF-av from the ecl-EKF. Only the Kalman gain is analyzed to save space. As shown in Figure 14, the IEKF-av has a more convergent Kalman gain, while that of the ecl-EKF exhibits periodic fluctuation, which depends on the attitude change in the quadrotor, like the results shown in Section 5.1.

Table 7. Comparison of Kalman gain for estimation of attitude and velocity in quadrotor flight test.

Kalman Gain Element	IEKF-av			ecl-EKF
	$\mathit{\mu }$	$\mathit{\sigma }$	$\frac{\mathit{\sigma }}{\left|\mathit{\mu }\right|}$	$\mathit{\mu }$	$\mathit{\sigma }$	$\frac{\mathit{\sigma }}{\left|\mathit{\mu }\right|}$
$K_{q_x/z_{m_x}}$	−0.0232	0.0127	0.5488	$8.8141\times {10}^{-4}$	$7.9180\times {10}^{-4}$	0.8983
$K_{q_x/z_{m_y}}$	−0.5474	0.0850	0.1553	$1.7863\times {10}^{-4}$	0.0010	5.6959
$K_{q_x/z_{m_z}}$	−0.0911	0.0118	0.1296	$6.3377\times {10}^{-4}$	$5.4550\times {10}^{-4}$	0.8607
$K_{v_z/z_{m_x}}$	−0.0892	0.0157	1.6892	$4.5189\times {10}^{-4}$	0.0020	4.4642
$K_{v_z/z_{m_y}}$	−0.0207	0.1250	6.0382	$-4.3975\times {10}^{-4}$	0.0029	6.5964
$K_{v_z/z_{m_z}}$	0.0550	0.0931	1.6920	$-2.6585\times {10}^{-4}$	0.0014	5.2806
$K_{b_{{\omega }_y}/z_{m_x}}$	−0.5344	0.0508	0.0950	$2.0447\times {10}^{-6}$	$5.3013\times {10}^{-7}$	0.2593
$K_{b_{{\omega }_y}/z_{m_y}}$	−0.0157	0.0413	2.6255	$-2.2956\times {10}^{-7}$	$4.2771\times {10}^{-7}$	1.8632
$K_{b_{{\omega }_y}/z_{m_z}}$	0.3143	0.0290	0.0923	$-3.4652\times {10}^{-7}$	$1.3560\times {10}^{-6}$	3.9131

lists the results of a numerical analysis of the Kalman gain fluctuation. For all the Kalman gains except the Kalman gain ${\mathbf{K}}_{b_{{\omega }_y}}$ with respect to ${\mathbf{z}}_{m_y}$, the SM ratios for the IEKF-av are less than those for the ecl-EKF. Most of the Kalman gains for the IEKF-av are more convergent than those for the ecl-EKF. This is the same feature as already revealed by Table 5 in Section 5.1.

6. Conclusions

This paper evaluated the estimation performance and revealed the properties of the IEKF-based navigation method through flight experiments with UAVs, and particularly through a comparison with the ecl-EKF, which is one of the prevalent navigation filters for small UAVs with limited sensor measurements and computational capacity.

One of the distinctive features of the IEKF is its convergent Kalman gain, state error covariance, and estimation parameters such as measurement innovation and bias estimation. This is because the linearized process model and measurement model of the IEKF depend only on invariant quantities, and the invariant quantities depend on the angular rate and acceleration. If the angular rate and acceleration are constant, the invariant quantities are constant, and thus the linearized model is constant and the filter converges to a linear Kalman filter. Even if the angular rate or acceleration changes instantaneously, the Kalman gain and state error covariance converge again soon after the change. This property was verified in the Section 5.1 and Section 5.2. Section 5.2 also demonstrated that, without location and altitude measurements, the IEKF was able to yield attitude and velocity estimations that were comparable to the estimations by the ecl-EKF for which location and altitude measurements were utilized.

The IEKF can be derived using either the abstract Lie group methodology or matrix Lie group methodology [1,7]. This paper used the abstract Lie group-based derivation. For further research, it is recommended to derive the matrix Lie group-based formulation of the IEKF for the same problem considered in this paper. In addition, it is expected that the problems addressed through the matrix Lie group approach [7] can be solved by the abstract Lie group-based approach in the subsequent research.