Journal of Sensors

Volume 2017, Article ID 9560108, 9 pages

https://doi.org/10.1155/2017/9560108

## Linear Kalman Filter for Attitude Estimation from Angular Rate and a Single Vector Measurement

^{1}College of Aerospace Science and Engineering, National University of Defense Technology, Changsha 410073, China^{2}School of Automation, University of Electronic Science and Technology of China, Chengdu, China

Correspondence should be addressed to Jin Wu; moc.liamtoh@ctseu_uw_nij

Received 7 May 2017; Accepted 20 July 2017; Published 18 October 2017

Academic Editor: Mohannad Al-Durgham

Copyright © 2017 Shangqiu Shan et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

#### Abstract

In this paper, a new Kalman filtering scheme is designed in order to give the optimal attitude estimation with gyroscopic data and a single vector observation. The quaternion kinematic equation is adopted as the state model while the quaternion of the attitude determination from a strapdown sensor is treated as the measurement. Derivations of the attitude solution from a single vector observation along with its variance analysis are presented. The proposed filter is named as the Single Vector Observation Linear Kalman filter (SVO-LKF). Flexible design of the filter facilitates fast execution speed with respect to other filters with linearization. Simulations and experiments are conducted in the presence of large external acceleration and magnetic distortion. The results show that, compared with representative filtering methods and attitude observers, the SVO-LKF owns the best estimation accuracy and it consumes much less time in the fusion process.

#### 1. Introduction

Attitude estimation is a key methodology in many applications involving satellite control, unmanned aerial vehicles, inertial human motion tracking, and so forth [1–4]. It requires accurate and efficient estimation from various sensor measurements. In fact, attitude estimation can be done by using different filtering methods, for example, the Kalman filter (KF) and complementary filter (CF) [5]. As these filtering methods adopt various senses of evaluating errors, the performances are quite different. Among all, the KF follows the minimum mean squared error (MMSE) principle that can be proved to be optimal; hence it has been widely used [6, 7].

In the past decade, many filters have been designed to give attitude estimation based on specific sensors. These sensors mainly consist of accelerometer, magnetometer, and inclinometer [8]. For instance, Sabatini proposed an extended Kalman filter (EKF) design by means of inertial and magnetic sensing [9]. Based on the same sensor combination, Li and Wang proposed an effective Adaptive Kalman Filter (AKF) in order to make the filter more accurate under harsh environments [10]. Using estimation of external acceleration, Suh designed an indirect Kalman filter (IKF, [11]). With algebraic calculations, Valenti et al. proposed a much more simplified quaternion Kalman filter (AQUA q-KF) that fuses the gyroscope, accelerometer, and magnetometer together [12]. Not only can KF methods give the optimal attitude estimates, but they also can make the estimated attitude more smooth. Yet it provides us with the statistic information of the estimated attitude parameters [3].

Apart from KFs, the CF methods are studied widely as well since the KF methods can hardly be used on platforms with relatively low hardware configurations [13, 14]. As a matter of fact, when the Kalman gain matrix is set to a constant matrix, the KF turns out to be a fixed-gain complementary filter, also known as the limiting Kalman filter [15]. Using the gradient descent optimization, Madgwick et al. proposed a fixed-gain CF [16]. Although CF is much faster than KF, it owns different convergence speed for different motions and thus lowers the overall attitude estimation accuracy. In this case, related adaptive schemes are introduced by Tian et al. (Adaptive-Gain Orientation Filter, AGOF) [17], Marantos et al. (Adaptive Wahba’s Complementary Filter, a-WCF) [18], and Vasconcelos et al. [19], which provide adaptive performances. Nonlinear CF is also developed by Mahony et al. using special orthogonal groups [20].

Above methods are mainly focused on specific sensor combinations and they need specific attitude determination. The multi-sensor-observation attitude determination can be solved with solutions to Wahba’s problem [21], which, leads to many efficient methods like QUaternion ESTimator (QUEST) [22], Fast Optimal Attitude Matrix (FOAM) [23], Singular Value Decomposition (SVD) [24], and and so forth [25, 26]. However, they require numerous costly matrix operations like adjoint, determinant, and eigendecomposition that make the batch processing slow. Wahba’s problem requires two or more vector observations because when there is only one vector observation, there would be two optimal associated eigenvectors [27]. In fact Valenti et al. have designed a new way for calculating attitude from a single accelerometer or magnetometer. However, the obtained quaternion is proved to be noncontinuous making it difficult to be fused with a gyroscope [12]. Besides, methods like Madgwick’s CF, AQUA q-KF, a-WCF, and AGOF may face a dilemma when one of the sensors fails. This is because the measurement information of these filters uses joint attitude determination or optimization which requires all the sensor data to be effective. Even one sensor’s failure may cause the attitude estimation to be incorrect or singular. To prevent the attitude angles from catastrophes, this paper aims to perform attitude estimation from angular rates and a single vector observation with better accuracy and less time consumption. The main contributions are listed below:(1)The stable attitude determination solution from a single sensor observation is obtained with some findings in our previous work [28].(2)A novel LKF scheme that is the Single Vector Observation Linear Kalman filter (SVO-LKF) is designed and investigated to fuse the angular rates and a single vector observation together.(3)Variance analysis of the state model and measurement model is performed which shows some interesting properties. An enhanced variance determination is designed to ensure the stability of the SVO-LKF.(4)Several experiments are carried out to verify the effectiveness of the proposed SVO-LKF. Some harsh conditions are added. Comparisons with representative methods are demonstrated with experiment results and related discussion.

This paper is briefly structured as follows: Section 2 contains the attitude determination using a single sensor observation. Section 3 includes the novel design of the SVO-LKF. Simulations and experiments are given in Section 4 in order to show comparisons of various aspects of the proposed SVO-LKF between representative methods. Section 5 contains the concluding remarks.

#### 2. Single Sensor Attitude Determination

This section deals with the attitude determination from a single sensor observation. Given a sensor observation in the body frame, the relationship with its corresponding vector observation in the reference frame can be written aswhere denotes the reference vector and is the direction cosine matrix (DCM). In [29], we developed a method using the decomposition of the DCM, which can be given with quaternionswith the parameters ofIn this section, the theory is extended to arbitrary sensor with exactly the similar approach.

Inserting (2) into (1) givesIt has been proved that where stands for the Moore-Penrose pseudo inverse. In fact, another property has been shown in the appendix of our recent contribution [29]: Also, in [30], we also show that, for a single vector observation, the following equation always holds:where can be given by the following: Note that, in [28], a similar equation is derived showing a new continuous solution to accelerometer attitude determination. Then, following such commitment, we can also obtain the solution from arbitrary single vector observation bywhere is a randomly chosen unit quaternion, if and only if . As a matter of fact, the square of can actually be computed by [30] Hence, (9) can be used as an approach for measuring quaternion from a single vector observation

#### 3. Design of the Novel KF Scheme

##### 3.1. Kalman Filter Basis

A discrete linear system without external control is constructed as follows [31]: where , denote the state vector and measurement vector, respectively. denotes the time epoch. defines the transition matrix from epoch to epoch . denotes the measurement matrix at epoch . and stand for the white Gaussian process noise and measurement noise respectively, such that where denotes the variance or covariance matrix. Using the following classical Kalman filtering process, we can recursively achieve the estimation [32] where denotes the Kalman gain matrix.

##### 3.2. Filter Design

The attitude quaternion is chosen as the state vector. The 1st-order quaternion kinematic equation is transformed into the following state model [33]: where is formed by the angular velocity and is the sampling time for th epoch. The noise item can be given by whereand stands for the gyroscopic noise with the auto-covariance of . Hence the auto-covariance of can be calculated by [12]

The quaternion from a strapdown sensor can be used as the measurement vector: In this paper, the measurement quaternion is obtained via (9)’s mutant; that is, it can be obtained with where stands for the estimated quaternion at epoch . Obviously, the variance of the proposed observation model relies on the variance of the vector observation and the variance of the estimated quaternion at last time epoch. Seen from the above equation, there are also nonlinearities inside it since there are multiplication items between and . In this case, the only way to derive the observation variance is to linearize the above equation. Here we define as the Jacobian matrix of , with respect to . Given the variances of vector observation and last estimated quaternion , , respectively, we can compute (11). The ranks of the two matrices are analytically computed as 3 and 2. That is to say, although the observation variance has correlation with last estimated quaternion, the final computed variance has much more relationship with the vector observation. Hence, in the current presented algorithm, the correlation between the process and observation models can be ignored. In this way the observation variance is approximated as The overall calculation procedure of the proposed scheme is shown in Algorithm 1.