3.3.1 DCM1-Based

Algorithms based on the DCM directly update the components of the matrix, avoiding the representation by means of Euler angles. The time variation of the elements of the DCM is given by (3.5). Although the whole matrix could be updated, the last column (denoted by ${\mathit{r}}_3$) suffices when providing information about ϕ and θ, as can be seen in (3.4). Using the common notation in control theory, the state vector is denoted by $\mathit{x}={\mathit{r}}_3$, and the system is described by

$\begin{array}{cccc}\hfill \dot{\mathit{x}}& ={[\overline{\mathbf{\Omega }}]}_{\times }\mathit{x}+{\mathit{\lambda }}_{\mathit{\xi }},\hfill & \hfill \mathbb{E}[{\mathit{\lambda }}_{\mathit{\xi }}{{\mathit{\lambda }}_{\mathit{\xi }}}^T]& =\mathbf{Q}=q_r\mathbf{I},\hfill \\ \hfill \mathit{y}& =-{\mathbf{I}}_3\mathit{x}+{\mathit{\lambda }}_{\mathit{v}},\hfill & \hfill \mathbb{E}[{\mathit{\lambda }}_{\mathit{v}}{{\mathit{\lambda }}_{\mathit{v}}}^T]& =\mathbf{R}={\sigma }_a^2{\mathbf{I}}_3.\hfill \end{array}$

Assuming that ${\sigma }_a^2$ can be identified by analyzing the static output of the accelerometers, there is only one tuning parameter, $q_r$, which determines the relative importance of the state propagation with respect to the information recovered from the accelerometers. Notice that this methodology results in a very simple filter, but it would imply the need of zero-velocity updates in practice as the bias of the gyroscopes is not explicitly estimated. For more details about this formulation see [7].

3.3.2 DCM2-Based

The previous filter can be modified to include the bias estimation [8]. According to the measurement model in (3.9), the bias-free angular velocity is given by $\tilde{\mathbf{\Omega }}=\overline{\mathbf{\Omega }}-{\mathit{\beta }}_{\mathrm{\Omega }}$. Denoting by $\mathit{x}={[{\mathit{r}}_3^T,{\mathit{\beta }}_{\mathrm{\Omega }}^T]}^T$ the state vector, the resulting system can be expressed as

$\begin{array}{cccc}\hfill \dot{\mathit{x}}& =\mathbf{A}(\mathit{x})\mathit{x}+{\mathit{\lambda }}_{\mathit{\xi }},\hfill & \hfill \mathbb{E}[{\mathit{\lambda }}_{\mathit{\xi }}{{\mathit{\lambda }}_{\mathit{\xi }}}^T]& =\mathbf{Q},\hfill \\ \hfill \mathit{y}& =\mathbf{C}\mathit{x}+{\mathit{\lambda }}_{\mathit{v}},\hfill & \hfill \mathbb{E}[{\mathit{\lambda }}_{\mathit{v}}{{\mathit{\lambda }}_{\mathit{v}}}^T]& =\mathbf{R},\hfill \end{array}$

where

$\begin{array}{c}\hfill \mathbf{A}(\mathit{x})=\left[\begin{array}{cc}\hfill {[\overline{\mathbf{\Omega }}-{\mathit{\beta }}_{\mathrm{\Omega }}]}_{\times }\hfill & \hfill \mathbf{0}\hfill \\ \hfill \mathbf{0}\hfill & \hfill \mathbf{0}\hfill \end{array}\right],\mathbf{C}=\left[\begin{array}{cc}\hfill -{\mathbf{I}}_3\hfill & \hfill \mathbf{0}\hfill \end{array}\right],\hfill \\ \hfill \mathbf{Q}=\left[\begin{array}{cc}\hfill q_r{\mathbf{I}}_3\hfill & \hfill \mathbf{0}\hfill \\ \hfill \mathbf{0}\hfill & \hfill q_{\beta }{\mathbf{I}}_3\hfill \end{array}\right],\mathbf{R}={\sigma }_a^2{\mathbf{I}}_3.\hfill \end{array}$

Notice that there is an extra parameter, $q_{\beta }$, accounting for the bias noise. It has a physical meaning as it describes the random walk model for the bias. Although used as a tuning parameter, it has to be small enough to allow only slow variations of the bias, which is known to happen in reality.

3.3.3 New DCM3-Based

In order to improve the angular velocity estimation, the above algorithm can be extended by including the angular velocity in the state vector, $\mathit{x}={[{\mathit{r}}_3^T,{\mathit{\beta }}_{\mathrm{\Omega }}^T,{\mathbf{\Omega }}^T]}^T$. The process model for the angular velocities is assumed to be constant, so they are only modified in the updating phase of the Kalman filter. In practice, this will work as a low-pass filter, controlled by the parameter $q_{\mathrm{\Omega }}$. In this case, the matrices describing the system are

$\begin{array}{c}\hfill \mathbf{A}(\mathit{x})=\left[\begin{array}{ccc}\hfill \mathbf{0}\hfill & \hfill \mathbf{0}\hfill & \hfill {[{\mathit{r}}_3]}_{\times }\hfill \\ \hfill \mathbf{0}\hfill & \hfill \mathbf{0}\hfill & \hfill \mathbf{0}\hfill \\ \hfill \mathbf{0}\hfill & \hfill \mathbf{0}\hfill & \hfill \mathbf{0}\hfill \end{array}\right],\mathbf{C}=\left[\begin{array}{ccc}\hfill -{\mathbf{I}}_3\hfill & \hfill \mathbf{0}\hfill & \hfill \mathbf{0}\hfill \\ \hfill \mathbf{0}\hfill & \hfill -{\mathbf{I}}_3\hfill & \hfill {\mathbf{I}}_3\hfill \end{array}\right],\hfill \\ \hfill \mathbf{Q}=\left[\begin{array}{ccc}\hfill q_r{\mathbf{I}}_3\hfill & \hfill \mathbf{0}\hfill & \hfill \mathbf{0}\hfill \\ \hfill \mathbf{0}\hfill & \hfill q_{\beta }{\mathbf{I}}_3\hfill & \hfill \mathbf{0}\hfill \\ \hfill \mathbf{0}\hfill & \hfill \mathbf{0}\hfill & \hfill q_{\mathrm{\Omega }}{\mathbf{I}}_3\hfill \end{array}\right],\mathbf{R}=\left[\begin{array}{cc}\hfill {\sigma }_a^2{\mathbf{I}}_3\hfill & \hfill \mathbf{0}\hfill \\ \hfill \mathbf{0}\hfill & \hfill {\sigma }_{\mathrm{\Omega }}^2{\mathbf{I}}_3\hfill \end{array}\right].\hfill \end{array}$

3.3.4 Euler EKF

In this algorithm six variables are estimated: three Euler angles and three biases of the gyroscopes. The state vector can be written as $\mathit{x}={[{\mathit{\eta }}^T{\mathit{\beta }}^T]}^T$. The nonlinear equations are described in a state-space form as

$\begin{array}{cccc}\hfill \dot{\mathit{x}}& =\mathit{f}(\mathit{x})+{\mathit{\lambda }}_{\mathit{\xi }},\hfill & \hfill \mathbb{E}[{\mathit{\lambda }}_{\mathit{\xi }}{{\mathit{\lambda }}_{\mathit{\xi }}}^T]& =\mathbf{q},\hfill \\ \hfill \mathit{y}& =\mathit{h}(\mathit{x})+{\mathit{\lambda }}_{\mathit{v}},\hfill & \hfill \mathbb{E}[{\mathit{\lambda }}_{\mathit{v}}{{\mathit{\lambda }}_{\mathit{v}}}^T]& =\mathbf{R},\hfill \end{array}$

where

$\mathbf{Q}=\left[\begin{array}{cc}\hfill q_{\eta }{\mathbf{I}}_3\hfill & \hfill \mathbf{0}\hfill \\ \hfill \mathbf{0}\hfill & \hfill q_{\beta }{\mathbf{I}}_3\hfill \end{array}\right],\mathbf{R}={\sigma }_a^2{\mathbf{I}}_3.$

The process equation is derived from (3.1) by directly incorporating the measurements from the gyroscopes as follows:

$\mathit{f}(\mathit{x})=\left[\begin{array}{c}\hfill ({\overline{\omega }}_x-{\beta }_{{\omega }_x})+({\overline{\omega }}_y-{\beta }_{{\omega }_y})\sin \varphi +({\overline{\omega }}_z-{\beta }_{{\omega }_z})\tan \theta \hfill \\ \hfill ({\overline{\omega }}_y-{\beta }_{{\omega }_y})\cos \varphi -({\overline{\omega }}_z-{\beta }_{{\omega }_z})-\sin \varphi \hfill \\ \hfill ({\overline{\omega }}_y-{\beta }_{{\omega }_y})\frac{\sin \varphi }{\cos \theta }+({\overline{\omega }}_z-{\beta }_{{\omega }_z})\frac{\cos \varphi }{\cos \theta }\hfill \\ \hfill 0\hfill \\ \hfill 0\hfill \\ \hfill 0\hfill \end{array}\right],$

while the measurement model is given by (3.12), i.e.,

$\mathit{h}(\mathit{x})=\left[\begin{array}{c}\hfill \sin \theta \hfill \\ \hfill -\sin \varphi \cos \theta \hfill \\ \hfill -\cos \varphi \cos \theta \hfill \end{array}\right].$

In this case the parameter $q_{\eta }$ determines how reliable the state propagation (based on gyroscopes measurements) is with respect to the observations made by the accelerometers.

3.3.5 Quaternions

Four quaternions and two biases of the gyroscope variables are estimated in this algorithm. The state vector can be written as $\mathit{x}={[{\mathbf{q}}^T,{\mathit{\beta }}^T]}^T$. The nonlinear equations are described as

$\begin{array}{cccc}\hfill \dot{\mathit{x}}& =\mathbf{A}(\mathit{x})\mathit{x}+{\mathit{\lambda }}_{\mathit{\xi }},\hfill & \hfill \mathbb{E}[{\mathit{\lambda }}_{\mathit{\xi }}{{\mathit{\lambda }}_{\mathit{\xi }}}^T]& =\mathbf{Q},\hfill \\ \hfill \mathit{y}& =\mathit{h}(\mathit{x})+{\mathit{\lambda }}_{\mathit{v}},\hfill & \hfill \mathbb{E}[{\mathit{\lambda }}_{\mathit{v}}{{\mathit{\lambda }}_{\mathit{v}}}^T]& =\mathbf{R},\hfill \end{array}$

where

$\begin{array}{c}\hfill \mathbf{A}(\mathit{x})=\left[\begin{array}{cc}\hfill \breve{\mathbf{\Omega }}\hfill & \hfill \mathbf{0}\hfill \\ \hfill \mathbf{0}\hfill & \hfill \mathbf{0}\hfill \end{array}\right],\mathbf{C}=\left[\begin{array}{cc}\hfill -{\mathbf{I}}_3\hfill & \hfill \mathbf{0}\hfill \end{array}\right],\\ \hfill \mathbf{Q}=\left[\begin{array}{cc}\hfill q_q{\mathbf{I}}_4\hfill & \hfill \mathbf{0}\hfill \\ \hfill \mathbf{0}\hfill & \hfill q_{\beta }{\mathbf{I}}_3\hfill \end{array}\right],\mathbf{R}={\sigma }_a^2{\mathbf{I}}_3.\end{array}$

The measurement model is given by the third column of (3.8), namely,

$\mathit{h}(\mathit{x})=\left[\begin{array}{c}\hfill 2(q_1{q}_3+q_2{q}_4)\hfill \\ \hfill 2(q_2{q}_3-q_1{q}_4)\hfill \\ \hfill q_1^2-q_2^2+q_3^2+q_4^2\hfill \end{array}\right].$

3.3.6 Comparative Real-Time Analysis in the Quanser Platform

The algorithms presented in the previous section have different levels of complexity, make use of different orientation representations, and even estimate different variables. A comparison was carried out in order to see if the different formulations have a significant impact on the estimation accuracy.

3.3.6.1 Experimental Platform and Devices

The experimental platform used for the comparative analysis is shown in Fig. 3.1. It is the 3D HOVER platform manufactured by Quanser and thought of as a test-bed platform of control algorithms for vertical take-off and landing (VTOL) vehicles, so that the translational degrees of freedom are clamped for convenience [9]. The Euler angles are measured by means of three optical encoders with an accuracy of 0.04 degrees, which can be sampled with a frequency up to 1000 Hz. These encoders provide a reliable pattern for the evaluation and comparison of the algorithms.

For the sake of comparison, two Inertial Measurement Units (IMU) have been used, the MicroStrain 3DM-GX2 and the Sparkfun Sensor Stick. The 3DM-GX2 has typical inertial sensor sets which are internally processed [10]. The angle estimations can be delivered in either Euler angles or rotational matrix at up to 250 Hz. The Sensor Stick is a hobbyist-level unit which consists of only a plain circuit board that gathers inertial sensors connected to a unique bus [11]. Some of the relevant information about these devices is shown in Table 3.1.

Table 3.1

IMUs specifications

	Microstrain 3DM-GX2	Sensor stick	MPU-6050
Size	41 × 63 × 32	35 × 10 × 2	21 × 17 × 2
Weight	50 g	1.2 g	6 g
Gyro range	±75 to ±1200 degree/s	±2000 degree/s	±250 to ±2000 degree/s
Gyro bias	±0.2 degree/s	Not defined	±20 degree/s
Gyro nonlinearity	0.2%	0.2%	0.2%
Gyro noise performance	0.17 degree/s (rms)a	0.38 degree/s (rms)	0.025 degree/s (rms)
Acc. bias	±5 mg (for ±5 g)	Not defined	±50 mg
Acc. nonlinearity	0.2%	0.5%	0.5%
Acc. noise performance	0.6 mg (rms)a	2.9 mg (rms)	1.3 mg (rms)

 ^a Measured from static output of the sensor.

3.3.6.2 Results

In order to evaluate the performance of the different algorithms, the platform described in Sect. 3.3.6.1 was used to follow alternating step references. These references were tracked using a PD controller. The angular estimates of the encoders and the 3DM-GX2 were stored, along with the information of gyroscopes and accelerometers of the Sensor Stick. The whole sampling procedure was implemented in a single real-time C++ code running at 100 Hz. This frequency was selected since it allows for a reasonable time to run the algorithms in a low-cost microcontroller. The estimation algorithms are executed offline using Matlab and the sampled data.

One of the experiments comparing the attitude measured by the encoders with that of the 3DM-GX2 and the one computed with the Euler EKF algorithm during an experiment with ±2 degree step references is presented in Fig. 3.2. At first sight the angular estimate obtained with the Sensor Stick is fast and noise-free, very similar to the estimate given by the 3DM-GX2.

Since it is very difficult to reach any conclusion from the naked eye observations, a set of five experiments like the one in Fig. 3.2 were carried out for different step amplitudes. The root mean square value (rmse) was chosen as a performance index, taking the encoder measurement as the ideal one. This index was averaged over the whole set of experiments for each step amplitude and for every algorithm. For each algorithm an optimization by trial and error was carried out to find the tuning of the parameters leading to the best performance.

The results of these experiments are depicted in Fig. 3.3 where the mean value of the rmse is represented along with its standard deviation. As can be seen, the accuracy of the estimation gets worse as the amplitude of the steps increases. All the algorithms perform very well for low angle oscillations.

However, for higher angles the 3DM-GX2 performs slightly better. This is sensible given that higher outputs of the sensors will manifest their imperfections, e.g., scale factor inaccuracy, nonlinearity, or cross-axis sensitivity. In any case, performance is very similar, and the comparison points out that a cheap board like the Sensor Stick and a simple linear KF provides sufficient accuracy for small angles.

Fig. 3.4 shows a situation in which the algorithms did not perform so well. It is possible to observe that one of the main sources of error is the deviation in a stationary position, far from the horizontal. One of the reasons for that is due to small nonlinearities of the accelerometers as they are calibrated using a linear model. There is also another effect, which is not so obvious. The estimation is slightly worse in one of the axes when there is velocity in the other. This is a consequence of the cross-sensitivity of the gyroscopes.

Among the evaluated algorithms, the DMC3-based is the only one which includes the angular velocities as estimated variables. Fig. 3.5 shows an example of how the estimation looks like, compared to the angular velocity extracted from the 3DM-GX2. The estimation exhibits less noise, and no delay is appreciated.

3.4.1 Simplified Algorithm

An advantage of the Euler formulation is that the yaw angle can be removed from the equations if the rotation matrix is well defined. Let us denote the vector of variables to be estimated by $\mathit{x}=[\varphi ,{\beta }_x,\theta ,{\beta }_y]$. It is reasonable to simplify the equations by assuming non-aggressive maneuvers, $\sin \alpha \approx \alpha$ and $\cos \alpha \approx 1$, with $\alpha =\{\varphi ,\theta \}$. From previous considerations, the kinematics of an aerial vehicle and the measurement model can be expressed by (3.1) and (3.12), respectively, which leads to the following linear equations:

$\begin{array}{cc}\hfill \dot{\mathit{x}}& =\underset{\mathbf{A}}{\underbrace{\left[\begin{array}{cccc}\hfill 0\hfill & \hfill -1\hfill & \hfill 0\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill -1\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill \end{array}\right]}}\mathit{x}+\underset{\mathbf{B}}{\underbrace{\left[\begin{array}{cc}\hfill 1\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill 1\hfill \\ \hfill 0\hfill & \hfill 0\hfill \end{array}\right]}}\mathit{u}+{\mathit{\lambda }}_{\mathit{w}},\hfill \\ \hfill \hat{\mathit{y}}& =\underset{\mathbf{H}}{\underbrace{\left[\begin{array}{cccc}\hfill 0\hfill & \hfill 0\hfill & \hfill 1\hfill & \hfill 0\hfill \\ \hfill -1\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill \end{array}\right]}}\mathit{x}+{\mathit{\lambda }}_{\mathit{v}},\hfill \end{array}$

where the input vector consists of the angular velocity measurements $\mathit{u}=[{\overline{\omega }}_x,{\overline{\omega }}_y]$, and $\mathit{y}=[{\overline{a}}_x,{\overline{a}}_y]$ contains the acceleration measurements. The third equation of the measurement model which involves the third accelerometer axis has been removed, as it has very low sensitivity with respect to the roll–pitch orientation for small angles.

These simplifications result in a smaller-size linear system, thus reducing the computational load substantially. Furthermore, it will be shown next how the structure of the matrices can also be exploited to reduce the Kalman filter to a set of simple equations. The continuous-time system (3.20) can be discretized with sample time $T_s$, assuming zero-order hold of the input, as follows:

$\begin{array}{cccc}\hfill {\hat{\mathit{x}}}_{k+1}& ={\mathbf{A}}_k{\hat{\mathit{x}}}_k+{\mathbf{B}}_k{\mathit{u}}_k+\mathit{\lambda }_{\mathit{w}}{}_k,\hfill & \hfill \mathbb{E}[\mathit{\lambda }_{\mathit{w}}{}_k{{\mathit{\lambda }}_{\mathit{w}}}_k^T]& ={\mathbf{q}}_k,\hfill \\ \hfill {\mathit{y}}_k& ={\mathbf{H}}_k{\hat{\mathit{x}}}_k+\mathit{\lambda }_{\mathit{v}}{}_k,\hfill & \hfill \mathbb{E}[\mathit{\lambda }_{\mathit{v}}{}_k{{\mathit{\lambda }}_{\mathit{v}}}_k^T]& ={\mathbf{R}}_k,\hfill \end{array}$

where

$\begin{array}{c}\hfill {\mathbf{A}}_k=e^{\mathbf{A}{T}_s}=\left[\begin{array}{cccc}\hfill 1\hfill & \hfill -T_s\hfill & \hfill 0\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill 1\hfill & \hfill 0\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 1\hfill & \hfill -T_s\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 1\hfill \end{array}\right],{\mathbf{B}}_k=\left(\underset{0}{\overset{T_s}{\int }}{e}^{\mathbf{A}s}\mathrm{d}s\right)\mathbf{B}=\left[\begin{array}{cc}\hfill T_s\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill T_s\hfill \\ \hfill 0\hfill & \hfill 0\hfill \end{array}\right],\hfill \\ \hfill {\mathbf{H}}_k=\left[\begin{array}{cccc}\hfill 0\hfill & \hfill 0\hfill & \hfill 1\hfill & \hfill 0\hfill \\ \hfill -1\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill \end{array}\right].\hfill \end{array}$

Notice that, as a consequence of the simplifications, the resulting system is decoupled. Therefore, two different Kalman filters could be separately implemented for roll and pitch dynamics. Furthermore, one can take advantage of the fact that matrix P is symmetric, so that only three of its entries need to be stored in the memory. Using (3.13) and (3.14), the following equations for the Kalman filter of the roll angle can be derived:

$\begin{array}{ccc}\hfill {\hat{\varphi }}_k^{-}& \hfill =\hfill & {\hat{\varphi }}_{k-1}+T_s({\overline{\omega }}_x-{\beta }_{x_k}),\hfill \\ \hfill {\beta }_{x_k}^{-}& \hfill =\hfill & {\beta }_{x_{k-1}},\hfill \\ \hfill p_{{11}_k}^{-}& \hfill =\hfill & p_{{11}_{k-1}}-2T_s{p}_{{12}_{k-1}}+T_s^2{p}_{{22}_{k-1}}+q_{{11}_k},\hfill \end{array}$

$\begin{array}{ccc}\hfill p_{{12}_k}^{-}& \hfill =\hfill & p_{{12}_{k-1}}-T_s{p}_{{22}_{k-1}},\hfill \\ \hfill p_{{22}_k}^{-}& \hfill =\hfill & p_{{22}_{k-1}}+q_{{22}_k},\hfill \\ \hfill {\hat{\varphi }}_k^{+}& \hfill =\hfill & (1-{\alpha }_{\varphi }){\hat{\varphi }}_k^{-}-{\alpha }_{\varphi }{\overline{a}}_y\hfill \\ \hfill {\beta }_{x_k}^{+}& \hfill =\hfill & {\beta }_{x_k}^{-}-{\gamma }_{\varphi }({\overline{a}}_y+{\hat{\varphi }}_k^{-}),\hfill \\ \hfill p_{{11}_k}^{+}& \hfill =\hfill & (1-{\alpha }_{\varphi })p_{{11}_k}^{-},\hfill \\ \hfill p_{{12}_k}^{+}& \hfill =\hfill & (1-{\alpha }_{\varphi })p_{{12}_k}^{-},\hfill \\ \hfill p_{{22}_k}^{+}& \hfill =\hfill & -{\gamma }_{\varphi }{p}_{{12}_k}^{-}+p_{{22}_k}^{-},\hfill \end{array}$

where

${\alpha }_{\varphi }=\frac{p_{{11}_k}^{+}}{p_{{11}_k}^{+}+r_{11}},{\gamma }_{\varphi }=\frac{p_{{12}_k}^{+}}{p_{{11}_k}^{+}+r_{{11}_k}}.$

In a similar way, the derivation of the Kalman filter equations for the pitch angle leads to the following set of equations:

$\begin{array}{ccc}\hfill {\hat{\theta }}_k^{-}& \hfill =\hfill & {\hat{\theta }}_{k-1}+T_s({\overline{\omega }}_x-{\beta }_{x_k}),\hfill \\ \hfill {\beta }_{y_k}^{-}& \hfill =\hfill & {\beta }_{y_{k-1}},\hfill \\ \hfill p_{{11}_k}^{-}& \hfill =\hfill & p_{{11}_{k-1}}-2T_s{p}_{{12}_{k-1}}+T_s^2{p}_{{22}_{k-1}}+q_{{33}_k},\hfill \end{array}$

$\begin{array}{ccc}\hfill p_{{12}_k}^{-}& \hfill =\hfill & p_{{12}_{k-1}}-T_s{p}_{{22}_{k-1}},\hfill \\ \hfill p_{{22}_k}^{-}& \hfill =\hfill & p_{{22}_{k-1}}+q_{{44}_k},\hfill \\ \hfill {\hat{\theta }}_k^{+}& \hfill =\hfill & (1-{\alpha }_{\varphi }){\hat{\theta }}_k^{-}+{\alpha }_{\varphi }{\overline{a}}_y,\hfill \\ \hfill {\beta }_{y_k}^{+}& \hfill =\hfill & {\beta }_{y_k}^{-}+{\gamma }_{\varphi }({\overline{a}}_x-{\hat{\theta }}_k^{-}),\hfill \\ \hfill p_{{11}_k}^{+}& \hfill =\hfill & (1-{\alpha }_{\theta })p_{{11}_k}^{-},\hfill \\ \hfill p_{{12}_k}^{+}& \hfill =\hfill & (1-{\alpha }_{\theta })p_{{12}_k}^{-},\hfill \\ \hfill p_{{22}_k}^{+}& \hfill =\hfill & -{\gamma }_{\theta }{p}_{{12}_k}^{-}+p_{{22}_k}^{-},\hfill \end{array}$

where

${\alpha }_{\theta }=\frac{p_{{11}_k}^{+}}{p_{{11}_k}^{+}+r_{{22}_k}},{\gamma }_{\theta }=\frac{p_{{12}_k}^{+}}{p_{{11}_k}^{+}+r_{{22}_k}}.$

3.4.2 Numerical Validation

The Kalman filter algorithm for estimating the roll and pitch angle (Eqs. (3.22)–(3.25)) has been first validated using the encoders of the Quanser platform. A commercial IMU (3DM-GX2) and a low cost inertial sensor (MPU6050) were also included in the platform in order to validate and compare the measurements. The MPU6050 is composed of a 3-axis gyroscope and a 3-axis accelerometer. It does not provide the angles of the rigid body, only the raw measurements of the sensors. The characteristics of the both devices are given in Table 3.1.

Since the angles of the experimental platform are unstable, a simple PD controller was used to stabilize the system to constant references of pitch and roll. The system was then perturbed by applying manual disturbances. All data was collected at 333 Hz, and the algorithm equations (3.22)–(3.25) were computed offline using Matlab. A trial and error tuning process resulted in the following covariance matrices:

$\begin{array}{ccc}\hfill {\mathbf{Q}}_k& =\left[\begin{array}{cccc}\hfill 0.94\cdot {10}^{-6}\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill 0.91\cdot {10}^{-6}\hfill & \hfill 0\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill \end{array}\right],{\mathbf{R}}_k\hfill & \hfill =\left[\begin{array}{cc}\hfill 0.37\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill 0.39\hfill \end{array}\right].\end{array}$

The estimation obtained by means of this procedure can be seen in Fig. 3.6. At first sight, it can be noticed that the proposed algorithm performs fairly well, thus validating the simplifications made in its derivation.

As it is difficult to visually evaluate the quality of both estimations, three performance indexes were chosen, namely, the root mean squared error, the maximum absolute error, and the delay, all of them computed with respect to the estimate given by the encoders (which are assumed to provide a “real” measurement). Table 3.2 gathers the information of these indices for an experiment of several minutes where the system was again stabilized around zero and perturbed manually. Notice that the proposed algorithm performs even better than the 3DM-GX2.

Table 3.2

Performance indices

		rmse (degrees)	$|\text{error}{|}_{\text{max}}$ (degrees)	Delay (ms)
3DM-GX2	Roll	0.3	1.56	25
	Pitch	0.27	1.46
MPU 6050	Roll	0.14	0.72	15
	Pitch	0.19	0.91

The evolution of the bias estimate is shown in Fig. 3.7. The real bias of the gyroscopes was computed by averaging the first few seconds during which the system remains steady. It is possible to see how the estimated bias converges to the real value within a few seconds.

3.4.3 Flight Tests

Although the platform described above is a suitable scenario for numerical comparison, there are still some handicaps to overcome in real flight, like vibrations and linear accelerations. The estimation algorithm is finally tested online and onboard to compute the roll and pitch angles and to stabilize a quadrotor prototype.

3.4.3.1 UPV Quadrotor Prototype

The quadrotor prototype developed at the UPV (Universidad Politécnica de Valencia, Spain) is depicted in Fig. 3.8. It has a distance of 41 cm between rotors, weighs around 1.3 kg without battery, and is outfitted with the IMU MicroStrain 3DM-GX2 and MPU6050, among other sensors. The basic hardware consists of a MikroKopter frame, YGE 25i electronic speed controllers, RobbeRoxxy 2827-35 brushless motors, and $10\times 4.5$ plastic propellers. All the computations are made onboard using an Arduino Due which is based on an Atmel SAM3X8E ARM Cortex-M3 microcontroller running at 84 MHz, and an Igep v2 board running Xenomai real-time operating system at 1 GHz.

The Arduino Due is in charge of reading every sensor, computing the Kalman filter and attitude control algorithms, controlling the motor's speed, and sending the data to the Igep board. The control algorithm consists of a PD controller with nested saturations. The Igep board is only used for data storage and also serves as a WiFi bridge. Although the proposed Kalman filter takes only 2 ms to run, the main loop in the microcontroller runs at 333 Hz restricted by the communications with the 3DM-GX2 and Igep board.

3.4.3.2 Experiments

The quadrotor was controlled by adjusting roll and pitch angles using PD controllers computed with the estimated values $\hat{\varphi },\hat{\theta },\dot{\hat{\varphi }}$, and $\dot{\hat{\theta }}$. Furthermore, angular references were sent to the vehicle using a joystick. The yaw angle was stabilized separately with the measurement of the Microstrain sensor, also with a PD controller.

Due to the lack of a motion capture system, the in-flight attitude estimate of the proposed algorithm is compared to that of the 3DM-GX2. Fig. 3.9 shows the attitude estimate during one minute of flight. One can see that both estimates are very similar (an rmse of 2.7 degrees and 1.6 degrees in pitch and roll axes, respectively). Although the 3DM-GX2 is not a fully reliable pattern, it can be seen that the proposed algorithm provides a fast, noise-free, and drift-free estimate. Furthermore, it should be also noticed that the control is computed with the estimated attitude. The small oscillations around the equilibrium point evidence that the attitude and velocity estimates lead to a very good control performance.

The angular velocities are shown in Fig. 3.10. The estimated angular velocities consist of the raw gyroscope measurements corrected with the estimated biases. The estimation of the bias avoids the need of correcting the offset of the gyroscopes prior to each flight and allows operating over long time periods. This comparison shows that, in spite of the simplifications, the accuracy of the estimates is fairly good compared to that of a commercial device, at least for non-aggressive flights. Because of the low computational cost of the algorithm, the angular estimates exhibit a very small delay, which also improves control performance.