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

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 andmagnetic 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.


Introduction
Attitude estimation is a key methodology in many applications involving satellite control, unmanned aerial vehicles, inertial human motion tracking, and so forth [1][2][3][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  [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.

Single Sensor Attitude Determination
This section deals with the attitude determination from a single sensor observation.Given a sensor observation D 푏 = (퐷 푏 푥 , 퐷 푏 푦 , 퐷 푏 푧 ) ⊤ in the body frame, the relationship with its corresponding vector observation in the reference frame can be written as where ⊤ denotes the reference vector and C is the direction cosine matrix (DCM).In [29], we developed a method using the decomposition of the DCM, which can be given with quaternions C = P 1 q, P 2 q, P 3 q (2) with the parameters of In this section, the theory is extended to arbitrary sensor with exactly the similar approach.Inserting (2) into (1) gives It 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 W 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 by where q random is a randomly chosen unit quaternion, if and only if W 2 = I.As a matter of fact, the square of W can actually be computed by [30] Hence, ( 9) can be used as an approach for measuring quaternion from a single vector observation

Design of the Novel KF Scheme
3.1.Kalman Filter Basis.A discrete linear system without external control is constructed as follows [31]: where x, y denote the state vector and measurement vector, respectively.푘 denotes the time epoch.Φ 푘,푘−1 defines the transition matrix from epoch 푘−1 to epoch 푘.H 푘 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] x where G 푘 denotes the Kalman gain matrix.

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 Initialize: whilee no stop commands received do (1) where and v 푔푘 stands for the gyroscopic noise with the autocovariance of ). Hence the autocovariance 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 q푘−1 stands for the estimated quaternion at epoch 푘 − 1. 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 W and q.In this case, the only way to derive the observation variance is to linearize the above equation.Here we define J(A, B) as the Jacobian matrix of A, with respect to B. Given the variances of vector observation and last estimated quaternion Σ D  ,푘 , Σ q−1 , 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.
As J(q y  , D 푟 ) has the rank of 3, the measurement quaternion can only obtain an observability of maximum two Euler angles.Such finding copes with the analytic results from that in [30], as well.However, note that, in the end of each filtering update, the quaternion would be normalized, adding to an insurance that the quaternion should not be divergent.The same circumstance occurs in the variance of quaternion kinematic equation and is resolved by quaternion normalization, too.
In engineering practice, during filtering process, the covariance of the state may become negative definite, which would lead to filtering divergence.The reason is that when the algorithm is applied on embedded platform, the wordlength of the float-point numbers may cause numerical loss [34].Hence, the square root Kalman filter (SRKF, [35]) is where Var{⋅} stands for the variance and {A} c denotes matrix A's Cholesky factorization.For common tasks on 64-bit PC, it is not necessary to use such formulations.

Experiments, Simulations, and Results
In this section, we are going to carry out several experiments to verify the performances of the proposed filter with respect to some representative methods.In order to achieve high precision data acquisition, a hybrid experimental platform is designed.As can be seen from Figure 1, the designed hardware consists of several parts including the power source, inertial measurement unit (IMU), magnetometer, telemetry, and an embedded computer.The power source is a Li-Po battery with the capacity of 1500 mah and voltage of 14.8 V.
The IMU and magnetometer are integrated as the product 3DM-GX3-25 provided by Microstrain (c).It generates raw data of angular rate, acceleration, and magnetic sensing with a wide output rate from 50 Hz to 1000 Hz.Besides, this module also gives high precision Euler angles; that is, it can be used as a standard attitude and heading reference system (AHRS).
The Xbee S3B Pro telemetry with the frequency of 900 MHz is utilized for robust wireless data transmission.The embedded computer has the central processing unit (CPU) of an 4-core ARM-CortexA53-based Qualcomm 410c chip with the clock speed of 1.4 GHz.Such high-configuration platform ensures that different algorithms can be concurrently executed.Moreover, to make the visualization of the attitude estimation and data acquisition more intuitive, an upper monitor is designed (see Figure 2).The upper monitor can also log the raw data and different attitude estimation results, which are later analyzed using the MATLAB r2015b software.
Several representative methods are adopted to make comparisons with the proposed filter.Since the proposed filter is designed as 4-dimensional, another EKF with the same dimension of state variables is introduced [36].Not only KF, but other complementary filters are compared as well, for example, the nonlinear complementary filter proposed by Mahony et al. [20] and the linear complementary filter proposed by Madgwick et al. [16].Apart from these algorithms, to verify the performance of the proposed filter under harsh conditions, an adaptive KF named as the indirect Kalman filter (IKF) proposed by Suh is studied [11].During the whole experiments, the attitude outputs from the 3DM-GX3-25 are served as the reference.

Single Vector Observation from an
Accelerometer.In this subsection, the single vector observation is acquired from an accelerometer.First an experiment is conducted with normal motion.Data acquisition is carried out with the rate of 500 Hz.The raw data from the gyroscope and accelerometer is shown in Figure 3.
The parameters of the proposed filter are measured as Σ v  = 0.0001 × diag(1, 1, 1) and Σ D  = 0.01 × diag(1, 1, 1).The reference vector of the accelerometer is set to D 푟 = (0, 0, 1) ⊤ .With acquired raw data from sensors, the attitude estimation results from different sources are obtained using the designed embedded computer, which is depicted in Figure 4. Mahony's filter and Madgwick's filter gains are set to 퐾푝 = 0.5 and 훽 = 0.1, respectively.
As the accelerometer can only measure the roll and pitch angles, the yaw angle is not compensated for during the sensor fusion.This is indicated by the black arrow in Figure 4 showing the drift of the yaw angles.Thus the estimated yaw angles with various algorithms are basically the same.
However, the roll and pitch angles are always compensated for by the accelerometer.In this figure, we can see that IKF is the worst intuitively.The attitude curves of the other algorithms are mixed together; that is, these algorithms have very similar estimation abilities of the roll and pitch angles.Compared with the reference angles from 3DM-GX3-25, the static rootmean-square errors (RMSEs) of various algorithms are listed in Table 1.
The statistics show that the proposed filter owns the best static mean errors of angles compared with other algorithms.However, a filter's performance should be tested under some harsh conditions, for example, in the presence of large external acceleration.In this case, the motion is generated with hand on a table.The raw data collected are shown in Figure 5.
The parameters of the filter are the same with that of the previous experiment.Attitude estimation results are depicted in Figure 6.In fact since the motion is generated on a horizontal table, the real roll and pitch angles should be approximately 0. However, as the accelerometer is a very important signal source for AHRS, even the estimated angles from 3DM-GX3-25 contain large errors ranging from 0 to ±20 degrees.Similarly, the behaviours of EKF, IKF, Mahony's filter, and Madgwick's filter are significantly impacted by external acceleration.The proposed SVO-LKF, however, remains relatively smooth compared with other filters.This is because the proposed filter is free of approximation of state and measurement models; that is, it is very accurate mathematically.Hence, attitude estimation from SVO-LKF is more accurate than EKF.Besides, the KF can dynamically calculate the Kalman gain matrix while, for the two adopted complementary filters, the gains are fixed.Consequently, the dynamic performances of these complementary filters are not comparable with the proposed SVO-LKF.The RMSEs of angles with respect to 0 and from different sources are listed in Table 2.The results show that the proposed filter owns the best attitude estimation performance under dynamic conditions.

Time Consumption. Time consumption significantly
determines the characteristic of a certain algorithm when it is applied on different platforms.Here, we would like to  study the time consumption performances of different filters.Using the samples from the experiment in the presence of large external acceleration, we obtain the time consumption information in Figure 7.We can find out that the proposed SVO-LKF is much more computationally cheap compared with other KF algorithms.This is because the filter is designed to be linear which uses the common KF procedures.However, EKF consumes more time on the calculation of linear approximation and noise propagation, which makes it slower than SVO-LKF.IKF is designed to be a 9-dimensional filter; hence the matrix operations are far more than that of the SVO-LKF.The internal reason that the proposed SVO-LKF is faster than previous algorithms is that the measurement model is instantly obtained via linear matrix multiplications.For the EKF, the linearization consumes a lot during the filtering update.For the IKF, not only does it have 6-dimensional state vector (for just gyro and accelerometer), it also adopts the matrix factorization for detection of external acceleration as well.Hence the SVO-LKF is faster in this way; that is, it maintains a simplest formulation for KF based the quaternion only, which does not generate computation burden for other estimation.
From another aspect, we can see that SVO-LKF consumes more time than the two adopted complementary filters.We can also find out that the differences are not very large.Yet the SVO-LKF is proved to be the best among all these filters.Hence we think that although SVO-LKF is computationally expensive compared to complementary filters; it is well worth the advances of the accuracy.Besides, SVO-LKF can provide the stochastic information of the estimated quaternions, which is significant in some later applications like spacecraft control and quality monitoring.The mean time consumption of different filters is shown below (see Table 3).
The standard deviation of time consumption actually determines the execution stability of the attitude estimation system.If the software is implemented on embedded platforms with real-time operating system, for example, FreeRTOS and ucOS, such behaviour would significantly influence the overall scheduling of the system.We can see from the above table that the proposed SVO-LKF is the best among compared Kalman filters.It is also the comparable with other complementary filters on this aspect as well, which ensures the robustness of SVO-LKF in applications.

Conclusion
Based on our previous work, this paper solved the attitude determination from a single vector observation.A novel linear Kalman filter (SVO-LKF) is designed to fuse the gyroscope's data and the single sensor observation together.
The quaternion kinematic equation is used as the state model while the quaternion attitude determination from a single vector observation is treated as the measurement.
Throughout analysis, we obtain the variance information of different models.
Several experiments are designed including the gyroscope-accelerometer and gyroscope-magnetometer combination.The experiments not only show the performances of the SVO-LKF along with other representative methods, but also give the details of the performances in the presence of large external acceleration.Related results are thoroughly discussed which shows that the proposed SVO-LKF is accurate in both static and dynamic conditions.Also when facing magnetic distortion, it basically remains uninfluenced compared to other filters.Finally, time consumption of various algorithms is given which shows that the SVO-LKF is the fastest one compared to other KF schemes.Although it is more time-costly than complementary filters, the advances of the accuracy are well worth the loss of time.
This paper shows that, using a single vector observation, we can make the attitude estimation much more accurate.Could it be a motivation for attitude estimation with even more vector observations?We think this would be another task for us to study in the future.

Figure 1 :
Figure 1: Designed experimental platform for the test and verification of the proposed filter.

Figure 2 :
Figure 2: The upper monitor can effectively show the real-time attitude from SVO-LKF.

Figure 3 :Figure 4 :
Figure 3: Raw data from the gyroscope and accelerometer during a normal motion.

Figure 5 :
Figure 5: Raw data from the gyroscope and accelerometer in the presence of large external acceleration.

Figure 6 :
Figure 6: Attitude estimation results in the presence of large external acceleration.

Figure 7 :
Figure 7: Time consumption of different sources adopted.

Table 1 :
Static RMSEs of roll and pitch angles.

Table 2 :
Dynamic RMSEs of roll and pitch angles.

Table 3 :
Statistics of time consumption from various algorithms.