Using an Unscented Kalman Filter (UKF) as the nonlinear estimator within a Global Positioning System/Inertial Navigation System (GPS/INS) sensor fusion algorithm for attitude estimation, various methods of calculating the matrix square root were discussed and compared. Specifically, the diagonalization method, Schur method, Cholesky method, and five different iterative methods were compared. Additionally, a different method of handling the matrix square root requirement, the square-root UKF (SR-UKF), was evaluated. The different matrix square root calculations were compared based on computational requirements and the sensor fusion attitude estimation performance, which was evaluated using flight data from an Unmanned Aerial Vehicle (UAV). The roll and pitch angle estimates were compared with independently measured values from a high quality mechanical vertical gyroscope. This manuscript represents the first comprehensive analysis of the matrix square root calculations in the context of UKF. From this analysis, it was determined that the best overall matrix square root calculation for UKF applications in terms of performance and execution time is the Cholesky method.
The improvement of microprocessors and sensors has increased civilian use of Unmanned Aerial Vehicles (UAVs) for various applications, many of which requiring an accurate estimate of the aircraft attitude [
The Unscented Kalman Filter (UKF) [
The selection of matrix square root operation in the UKF differs among authors. Julier and Uhlmann state that “
This paper aims to expand upon the existing matrix square root comparison studies through an example application of the matrix square root within a UKF-based GPS/INS sensor fusion algorithm for attitude estimation that relies on experimentally collected flight data. By analyzing different matrix square root methods in the context of the UKF, a matrix square root is required at each discrete time step in the algorithm, allowing for a more general comparison since many different matrices are considered. In addition this recursive filtering application introduces the effects of the propagation of uncertainties in the matrix square root computation. Furthermore, the flight data used for this study were selected from a large library of data in order to obtain diversity with respect to different flight conditions, thus providing an additional level of generalization. The use of experimental data for this study provides a necessary empirical approach to a problem that has previously only been considered in a theoretical context. To evaluate the performance of the attitude estimation algorithm, the aircraft pitch and roll estimates are compared with measurements acquired independently from the IMU and GPS with a high-quality mechanical vertical gyroscope. This sensor fusion study builds upon previous work conducted by various West Virginia University (WVU) researchers [
The work presented herein represents the first comprehensive analysis of matrix square root calculations used for UKF applications. This study utilizes a diverse set of experimental flight data to analyze the performance speed and accuracy of different matrix square root calculations in the context of a UKF-based GPS/INS sensor fusion algorithm. The rest of the paper is organized as follows. Section
Formulations of the UKF for GPS/INS sensor fusion are well known and can be found in a number of publications [
The UKF uses an unscented transformation [
The prediction step for the UKF consists of passing each state sigma point through the state prediction equations:
An important requirement of the UKF algorithm is the calculation of the square root of the state covariance matrix,
If
Another common matrix square root method is the Schur method, which uses the Schur decomposition:
In addition to analytical methods for calculating a matrix square root, various iterative methods have been derived. One of the most common iterative methods is Newton’s method, which can be written as
All of the matrix square root algorithms discussed up to this point are methods of calculating the principle square root of a matrix. Another form of the matrix square root is found using the Cholesky decomposition [
The computational complexity of the matrix square root operation is important for use in the UKF because it is a significantly expensive part of the UKF algorithm [
Matrix square root algorithm computational requirement summary.
Algorithm | FLOPs |
---|---|
Diagonalization | |
Schur | |
Newton’s iteration | |
Denman-Beavers (DB) iteration | |
Product DB iteration | |
Cyclic Reduction (CR) iteration | |
IN iteration | |
Cholesky |
It is shown in Table
The sensor fusion formulation considered for this study has states given by the local Cartesian position
An Inertial Measurement Unit (IMU) provides measurements of the linear accelerations as specific force,
The IMU measurements of roll, pitch, and yaw rates,
The state equations (
Measurements from GPS are used to calculate the position and velocity of the receiver in the Earth-Centered-Earth-Fixed (ECEF) coordinate frame. These calculations are done within the GPS receiver. For navigation purposes, the position and velocity are transformed into a local Coordinate frame with origin at the WVU flight testing facility. Therefore, the GPS is used to obtain calculations of local position,
The results of this study were derived using an off-line postprocessing analysis of previously recorded flight data. The attitude estimation algorithm using UKF was coded manually in-house using MATLAB. Different versions of the attitude estimation code were created for each matrix square root calculation, keeping all other portions of the code constant. All postprocessing was done on the same HP Pavilion Elite HPE Series desktop PC with Intel CORE i5 processor, which has processor speed of 2.67 GHz and 6 GB RAM.
This sensor fusion study was conducted using flight data collected on the three WVU YF-22 research UAVs (Green, Red, and Blue), shown in Figure
WVU YF-22 research UAVs.
Flight data were collected with two different avionics system architectures. Avionics system number1 [
Inertial measurement unit (IMU) specifications.
Inertial measurement unit (IMU) | Crossbow VG400CA-200/VG400CC-200 | Crossbow DMU(VG400)-100 | Analog devices ADIS-16405 |
---|---|---|---|
Resolution | 16-bit | 16-bit | 14-bit |
Rate gyroscope sensitivity | ±200°/s | ±100°/s | ±150°/s |
Accelerometer sensitivity | ±10 g | ±8 g | ±18 g |
Rate gyroscope random walk | 1.7°/h1/2 | 0.85°/h1/2 | 2.0°/h1/2 |
Accelerometer random walk | 0.5 m/s/h1/2 | 0.1 m/s/h1/2 | 0.2 m/s/h1/2 |
Independent roll and pitch measurements for both systems were obtained with a Goodrich VG34 mechanical vertical gyroscope. The VG34 has a ±90° sensitivity on the roll axis and ±60° sensitivity on the pitch axis and is sampled with 16-bit resolution. The VG34 has a self-erection system and reported accuracy of within 0.25° of true vertical. The output of the mechanical vertical gyroscope is used as the “truth data” for the sensor fusion study.
To evaluate the sensitivity of the UKF, 23 sets of flight data were selected from a large library of flight data collected at WVU. Data were selected from flights on the WVU YF-22 platform, which consists of three UAVs. Each data set consists of three-axis IMU measurements, GPS position, and velocity calculations in local geodetic coordinates, along with roll and pitch angle measurements from a mechanical vertical gyroscope. Flights were selected with the goal of obtaining a diverse set of data in order to fairly compare sensitivity effects. The distribution of the flight selection is summarized in Tables
Summary of the distribution of flight systems.
System type | Distribution of flights | ||
---|---|---|---|
UAV | (8) Green YF-22 | (5) Red YF-22 | (10) Blue YF-22 |
Avionics system | (21) Avionics system number 1 | (2) Avionics system number 2 | |
Control system | (12) Pilot only | (11) Pilot with sections of autonomous flight |
Summary of the distribution of flight conditions.
Approximate flight condition | Minimum | Maximum | Mean | Standard deviation |
---|---|---|---|---|
Temperature (°F) | 38 | 86 | 65.1 | 13.9 |
Wind speed (mph) | 0 | 11 | 5.8 | 3.5 |
Flight duration (sec) | 375 | 640 | 485 | 76.0 |
The performance of the UKF sensor fusion algorithm is evaluated from the estimated roll and pitch angles. These estimates are compared with independent measurements obtained from a high-quality mechanical gyroscope. The gyroscope data are considered to be a “truth” value, and therefore the difference between the sensor fusion estimate and the corresponding gyroscope measurement represents the error. To quantify this error, the mean of the absolute value of the error and the standard deviation of the error are considered for both roll and pitch. To simplify performance comparisons, a scalar composite performance cost,
To analyze the sensitivity of this formulation of GPS/INS sensor fusion to the matrix square root operation, the UKF algorithm was executed for each set of flight data using different methods of calculating the matrix square root. In particular, the diagonalization method, Schur method, Cholesky method, and five different iterative methods were implemented. For each of the iterative methods, the UKF was executed for each set of flight data using a set number of iterations throughout the entire flight. This process was repeated for the number of iterations ranging from 5 to 20 by unit increments. For each individual case, results were evaluated based on performance cost,
Performance cost of UKF for different matrix square root operations.
UKF computational requirements for different matrix square root operations.
In Figure
Since computational requirements are also important for a matrix square root algorithm, the actual execution time of the entire UKF algorithm was calculated for each of the different methods. These execution times are intended to provide an approximate empirical verification of the theoretical FLOP estimates in order to compare the computational requirements of the different algorithms. All execution time analyses were conducted using the same computer under approximately the same operating conditions. A mean of these execution times over each of the 23 flights is shown in Figure
It is important to note that the number of FLOPs estimate is for the matrix square root operation only, while the execution times represent the run times of the entire sensor fusion algorithm. However, this is representative of the overall trend since the only difference between the curves is the matrix square root operation used. Because the diagonalization, Schur, and Cholesky methods do not require iterations, these algorithms are represented in Figure
In order to compare accuracy of the actual matrix square root calculation itself, the
Matrix square root operation accuracy.
Figure
Relationship between matrix square root accuracy and UKF performance.
As shown in Figure
A different method of handling the square root requirement of the UKF, named the “square-root UKF (SR-UKF)”, was suggested by van der Merwe and Wan [
Comparison of UKF and SR-UKF performance.
The mean performance costs of the two different methods are shown in Table
Comparison of UKF and SR-UKF.
Matrix square root method | Mean performance cost | Mean execution time (sec) |
---|---|---|
UKF (Cholesky) | 1.795 | 8.16 |
SR-UKF | 1.996 | 5.73 |
It is also interesting to note from Figure
This paper presented a comparison of different matrix square root calculations within the UKF. The GPS/INS sensor fusion attitude estimation problem for UAV applications was used as an example to evaluate the performance with respect to matrix square root accuracy, computational cost, and attitude estimation performance. In terms of attitude estimation performance, the Cholesky, diagonalization, and Schur methods yielded the highest accuracy; however this same performance can be reached using a sufficient number of iterations in any of the iterative methods. Newton’s iteration was found to diverge in certain instances and is therefore not recommended for UKF applications. The cyclic reduction (CR) iteration demonstrated the fastest performance convergence of the iterative methods. In terms of execution time, the SR-UKF is computationally efficient, but at the cost of performance. Overall, the Cholesky method was found to provide the best compromise in terms of both performance and execution time.
For real-time applications of the UKF, such as attitude estimation for small UAVs, computation is an important consideration factor. For most cases, the Cholesky method is the best suited matrix square root method due to its fast execution and high accuracy. If computational cost is even more important than the accuracy of the filter, the SR-UKF could be considered. The diagonalization and Schur methods are acceptable approaches for off-line applications, because the accuracy is similar to the Cholesky method, although they require more computation time. These methods also might be more desirable than the Cholesky method because they provide a more intuitive representation of the matrix square root, that is, the principle square root. Any of the iterative methods, except for Newton’s iteration, could also be used with a sufficient number of iterations, though these methods are a bit less intuitive.
This research was partially supported by NASA Grant no. NNX07AT53A and Grant no. NNX10AI14G.