Abstract
A novel H∞ filter called square-root cubature H∞ Kalman filter is proposed for attitude measurement of high-spinning aircraft. In this method, a combined measurement model of three-axis geomagnetic sensor and gyroscope is used, and the Euler angle algorithm model is used to reduce the state dimension and linearize the state equation, which can reduce the amount of calculation. Simultaneously, the method can be applied to the case of measurement noise uncertainty. By continuously modifying the error limiting parameters to update the measurement noise estimation, the filtering accuracy and robustness can be improved. The square-root forms enjoy a consistently improved numerical stability because all the resulting covariance matrices by QR decomposition are guaranteed to stay positive semidefinite. The algorithm is applied to the simulation experiment of attitude measurement with the combination of geomagnetic sensor and gyroscope and compared with the results of Unscented Kalman filter, cubature Kalman filter, square root cubature Kalman filter, and singular value decomposition cubature Kalman filter, which proves the effectiveness and superiority of the algorithm.
1. Introduction
The objective of high-spinning aircraft attitude measurement is to determine the orientation of a body-fixed coordinate frame with respect to a reference coordinate frame [1]. Since the attitude estimation is essentially a nonlinear filtering problem, many nonlinear filtering methods have been proposed. The representative nonlinear filters are extended Kalman filter (EKF), unscented Kalman filter (UKF), and cubature Kalman filter (CKF). EKF expands the nonlinear state process and measurement model into the Taylor series around the filtering value and omits the second-order term to obtain the first-order approximate linearized model [2]. Then, the system is filtered according to the principle of linear Kalman filter [3]. The premise of this method is that the nonlinear function has practical expression and partial derivative. The Jacobian matrix obtained by the first-order approximation has errors in the actual model, and the errors will accumulate in the process of system model transfer, resulting in the divergence of the final calculated value [4]. Julier et al. [5] abandoned the traditional method of linearization of a nonlinear function, used the unscented transformation (UT transformation) to approximate the probability density distribution of nonlinear function to deal with the nonlinear transfer problem of mean and covariance, and proposed the UKF algorithm. Similar to UKF, CKF uses spherical radial rule to approximate the probability density distribution of nonlinear function [6]. Although UKF and CKF come from different mathematical viewpoints, they can be compared from several aspects. Both of them use deterministic sigma points to obtain mean and covariance. UKF needs 2n+1 sigma points, while CKF needs 2n cubature points. UKF needs to add a scaling parameter related to the state dimension, while CKF does not need other parameters in the sampling process. No matter what the state dimension is, the weight can always be kept positive, so that the filtering can go on smoothly. Therefore, CKF is a better application than UKF in state estimation of the high-dimensional system [7].
The standard CKF algorithm after repeated recursion, the rounding error will gradually accumulate, resulting in the loss of symmetry and positive definiteness of the error covariance matrix [8]. The cumulative error has appeared with high frequency in the attitude measurement of aircraft. In recent years, many scholars have done a lot of research on this phenomenon. Among them, Qiu and Guo [9] introduce the adaptive Huber algorithm based on multiple strong tracking into the standard CKF, which can effectively improve the filtering performance of CKF; Geng et al. [10] overcome the influence of CKF model error and abnormal interference by establishing an adaptive factor based on prediction residual; Tang et al. [11] use QR decomposition of matrix and quaternion numerical integration to construct a new CKF algorithm, which effectively improves the rate of convergence of filtering; Huang et al. [12] proposed a stable and high-strength tracking CKF algorithm to improve the regulation ability of CKF. To reduce the complexity of filtering operation, the QR decomposition method with strong numerical stability is applied to decompose and iterate the system state covariance matrix to ensure numerical stability of matrix operation in the algorithm [13]. H∞ filtering is a linear state estimation method based on the assumption that the noise interference environment is the worst. It is suitable for the state estimation problem with unknown statistical characteristics of system measurement noise and has certain adaptability to inaccurate system modeling [14–16]. In this paper, a new improved SR-CKF algorithm based on H∞ filtering is proposed, which combines the robustness advantage of H∞ filtering with the high precision processing ability of SR-CKF in nonlinear system, and uses the combined system of three-axis geomagnetic sensor and gyroscope to calculate the flight attitude of aircraft.
The motivation of this paper is to propose a novel attitude estimation algorithm that not only provides reliable estimation accuracy, but also improves the numerical stability. Specifically, the main contributions of this work are as follows: (1) in linear-nonlinear measurement system, H ∞ filter is integrated into SR-CKF, and square-root cubature H∞ Kalman filter (SR-CH∞KF) is established; (2) Euler angle measurement model is used in SR-CH∞KF, and linear state equation is used to alleviate the amount of calculation; (3) the approximate methods of error covariance matrix and crosscovariance matrix are mastered from the work of Silbley et al. [17]. The H∞ filtering of linear state is successfully transformed into nonlinear system. Moreover, inspired by the approach, the error covariance matrix and the updated expression of measurement noise are derived in detail. Compared with the reference [18], the algorithm in this paper is simpler and reduces the computational complexity.
The rest of this paper is organized as follows. Section 2 introduces the configuration of sensors and attitude measurement model. The square-root cubature H∞ Kalman filter in the linear-nonlinear system is reviewed in Section 3. Section 4 describes the attitude estimation algorithm based on square-root cubature H∞ Kalman filtering. In Section 5, the filtering effect of SR-CH∞KF is compared with the other four kinds of filtering through simulation experiments. Finally, some concluding remarks are given in Section 6.
2. Sensors Configuration and Attitude Measurement Model
2.1. Coordinate System and Sensor Configuration
Since the aircraft is symmetrically constructed and the center of gravity of the aircraft is on the axis, the sensor is installed on the axis, and the gyroscope and geomagnetic sensor are installed in front and back positions. The installation schematic diagram is shown in Figure 1 below.
In the figure above, o-XdYdZd is the carrier coordinate system, o-XcYcZc is the geomagnetic sensor coordinate system, and o1-XrYrZr is the gyroscope coordinate system. Xd, Xc, and Xr are installed in the same direction as the axis of the aircraft. Yr and Yc are installed in parallel. Zr and Zc are installed in parallel. To better represent the flight attitude of the high-spinning aircraft in the flight process, we choose the North-Up-East coordinate system as the navigation coordinate system, and use N, S, and E to represent the north axis, up axis, and east axis of the coordinate system.
In the data transmission of the measurement unit, the data of the geomagnetic sensor and gyroscope are converted into digital quantity and transmitted to the main control board through the AD converter. The main control board completes the data solving process. This data transmission method can improve the frequency of sensor acquisition and improve the speed of the system.
2.2. Attitude Measurement Model of Gyroscope
The attitude change of high-spinning aircraft is expressed by the pitch angle, yaw angle, and roll angle of the aircraft in the North-Up-East coordinate system. The pitch angle is the angle between the axis of the aircraft and the horizontal plane, expressed by ; yaw angle is the angle between the projection of the axis on the horizontal plane and the north axis, expressed by ; roll angle is the angle of the aircraft rolling around its axis, expressed in (The direction of the three attitude angles is judged according to the right-hand rule). Any attitude of high spin aircraft during flight can be represented by the following schematic diagram.
In Figure 2, the initial attitude of the aircraft can be regarded as the complete coincidence of the carrier coordinate system and the North-Up-East coordinate system, and then, rotate angle around the -axis, angle around the -axis, and angle around the -axis.
The gyroscope can measure three angular velocities of the high-spinning aircraft in the carrier coordinate system, which are expressed by ωx, ωy, and ωz, respectively (the direction of the three angular velocities can be judged by the right-hand rule), where ωx is the angular velocity around the Xd axis, ωy is the angular velocity around the Yd axis, and ωz is the angular velocity around the Zd axis. The relationship between the three angular velocities measured by the gyroscope and the attitude angular velocity of the aircraft can be obtained by Euler angle differential equation as follows:
In the formula, are pitch angular velocity, yaw angular velocity, and roll angular velocity, respectively. By expanding the above formula, we can get the following formula.
The pitch angle, yaw angle, and roll angle can be obtained by numerical integration of pitch angular velocity, yaw angular velocity, and roll angle velocity. The mathematical model is as follows:
Due to the high-spinning speed of the high-spinning aircraft, it will exceed the range measured by the gyroscope, so the Zr axis of the gyroscope is in the open circuit state during the flight of the aircraft. The three axes of gyroscope are assembled, and the open circuit of one axis will not affect the normal operation of the other two axes. The roll angle of the previous moment needed in the calculation of pitch angle and yaw angle is measured by the geomagnetic sensor.
2.3. Attitude Measurement Model of Geomagnetic Sensor
As the basic energy field of the earth, the geomagnetic field is an important inherent resource on the earth. The geomagnetic intensity at any place near the ground is determined by geomagnetic elements, which are composed of longitude, latitude, and altitude, providing a good reference for geomagnetic navigation. According to the longitude, latitude, and altitude of the launch location, the magnetic inclination and declination of the geomagnetic vector in the North-Up-East coordinate system can be calculated through the world geomagnetic field model, and then, the three-axis components of the geomagnetic vector in the North-Up-East coordinate system can be obtained, expressed by , , and , as follows.
In the above formula, is the geomagnetic field intensity at the launch site of the aircraft; and are the magnetic declination angle and the magnetic inclination angle, respectively. The magnetic inclination angle above the horizontal plane is positive, and it is positive when the magnetic declination is in the north by west. According to the coordinate system transformation method shown in Figure 2, the relationship between the carrier coordinate system and the navigation coordinate system during the flight of the high-spinning aircraft can be expressed by the following formula:
The projection of the geomagnetic vector on the three axes of the carrier coordinate system during the flight of the high-spinning aircraft can be expressed as:
The coordinate system of geomagnetic sensor is completely coincident with that of carrier, that is, , , and . From the ratio relationship between Byc and Bzc, the analytical formula of roll angle can be obtained as follows:
When the geomagnetic sensor is used to measure the roll angle of the aircraft, it is necessary to use the geomagnetic sensor to calculate the roll number of the aircraft. The calculation method of the number of rolling turns is described in detail in the reference [19]. Since the roll angle ranges from 0° to 360° and the inverse tangent evaluation ranges from -180° to 180°, it is necessary to introduce other variables to redefine the solution range of roll angle. The roll angle is the angle of the aircraft rolling around its axis, which can be seen as the reading of the Zd axis of the carrier coordinate system turning around the axis. The roll angle can be judged by quadrants (as shown in Figure 3).
(a)
(b)
(c)
(d)
In Figure 3, Yc0 and Zc0 are the positions of Yc axis and Zc axis of geomagnetic sensor when the roll angle is 0°. (a) (b), (c), and (d) indicate that the roll angle ranges from 0°~90°, 90°~180°, 180°~270°, and 270°~360°, respectively. It can be expressed by mathematical expression: (a) , ; (b) , ; (c) , ; (d) , .
In the formula, is the actual roll angle, and is the measured number of turns of the aircraft. is directly solved by Formula (7), which can be expressed by mathematical model as follows:
3. Application of SR-CKF Based on Linear-Nonlinear System
In engineering application, the rounding error of cubature Kalman filter algorithm in recursive calculation may cause the error covariance matrix to lose symmetry and positive definiteness, which affects the numerical stability of the filter. The square-root cubature Kalman filter avoids the direct matrix square root operation by introducing orthogonal triangular decomposition, which can greatly improve the stability of the filter and the filtering performance of linear-nonlinear attitude measurement system.
According to the pitch angle and yaw angle obtained in the attitude measurement system of gyroscope/geomagnetic sensor, the equation of state is composed of the pitch angle and yaw angle measured by the gyroscope in Equation (3), and the measurement equation is composed of the -axis output expression of the geomagnetic sensor in Equation (6). The formula is as follows:
In the above equation, is the state vector of the system; is the measurement of the system; is control input matrix; is nonlinear function. and are Gaussian white noise with mean value of 0 and covariance of and for state system and observation system, respectively, and they are independent of each other.
The SR-CKF algorithm for linear-nonlinear systems is as follows:
(1) System initialization: the initial pitch angle and yaw angle are determined by the launch attitude, and the initial error covariance is determined by the actual launch conditions
(2) Calculate point set and mean:
In the formula, . is the number of basic cubature points. According to the third-order spherical radial volume criterion, the number of volume points is twice the dimension of state variables, that is, and is the dimension of state variables. expresses a complete set of fully symmetric points. Since the state variables are pitch angle and yaw angle, then . The th element in the point set is as follows:
(3) Time update equations:(i)Calculate the cubature points: where Chol is the Cholesky decomposition of the matrix.(ii)The propagation integral point is calculated, and the predicted value of the cubature points brought into the equation of state is calculated. (iii)State variable prediction, weighted sum of predicted values of all cubature points: (iv)Estimate the square root coefficient of the error covariance matrix:
In the formula, is the lower triangular matrix obtained by QR decomposition, and is the matrix obtained by Cholesky decomposition of the covariance moment of the state system noise.
(4) Measurement update equations:(i)Recalculate the cubature points: (ii)Measurement prediction and weighted sum of cubature points prediction values: (iii)Calculate innovation covariance matrix:
In the formula, is the covariance matrix of the state system noise obtained by Cholesky decomposition. (iv)Calculate the cross covariance matrix: (v)Calculate the filter gain of SR-CKF: (vi)Calculate the estimated value of state variables: (vii)Square root coefficient of update error covariance matrix:
4. Flight Attitude Estimation Based on H∞ Filter and SR-CKF
4.1. Fundamental of H∞ Filtering
For the uncertainty of system model and noise statistical characteristics, H∞ filtering introduces the idea of H∞ norm to minimize the H∞ norm from interference input to filtering error output, so as to minimize the estimation error of the system in the worst case of interference.
The cost function is defined as follows:
The standard symbolic operations used in the formula are as follows: .
The purpose of H∞ filter is to minimize the error of state estimation. In general, it is difficult to obtain the best analytical solution for the optimal H∞ filter problem. Therefore, the suboptimal iterative algorithm is sought. The boundary of J∞ is designed by Wk, Vk, and X0 in the worst case, and a threshold value is specified. where sup is the upper limit of the cost function and is the error limiting parameter. As designers, we expect to find the optimal so that can contain any interference of Wk, Vk, and X0, and minimize J∞ in the worst case. To solve the problem of H∞ filter, the following inequality must be satisfied at every moment, that is, Riccati inequality [20]: where is the error covariance matrix and is the -order identity matrix. is the Jacobian matrix expanded by at the estimated value . In the H∞ filter, the error limits the worst-case estimation error of the parameter . The smaller the is, the stronger the robustness of the system is; the larger the is, the closer the characteristic of the H∞ filter is to the standard Kalman filter.
4.2. Cubature H∞ Kalman Filter
The cubature H∞ Kalman filter (CH∞KF) is formed by integrating H∞ filter into cubature Kalman filter. In CH∞KF process, the time prediction stage is similar to CKF, including factor decomposition of error auxiliary matrix, estimation cubature points propagation and state prediction. Because the measurement equation is a nonlinear equation, it is necessary to use the extended Kalman filter (EKF) method to approximate the nonlinear equation into the linear equation by using Jacobian matrix. The update state and covariance matrix of H∞ filter in the measurement update stage can be expressed as:
is the estimated value of the observation noise. Based on the noise sample value, Liang et al. [21] gave the mathematical analytical formula of the estimated value of the observation noise:
Sibley et al. [17] used Jacobian matrix to transform nonlinearity into linearity and proposed error covariance and cross covariance approximation methods based on linear error propagation characteristics.
Using Formula (28) and Formula (29), the part of Formula (27) is transformed:
The update covariance matrix can be transformed into:
By taking Equations (28) and (29) into Equation (26), the update equation of gain value can be obtained as follows:
4.3. Square-Root Cubature H∞ Kalman Filter (SR-CH∞KF)
By bringing the above gain update, state estimation, and error covariance estimation into the filtering step of square-root cubature Kalman filter, the square-root cubature H∞ Kalman filter can be obtained as follows.
Before the judgment, the noise error estimate is calculated according to Formula (27), and then, the Riccati inequality in Formula (25) is transformed according to Formula (28) and Formula (29).
According to the actual situation, after selecting the appropriate error limiting parameters, the measurement update calculation is carried out. The analytical formula for calculating the gain of SR-CH∞KF filter is shown in Formula (32). The calculation of state update value can refer to Formula (26), and the update of error covariance matrix can refer to Formulas (30) and (31).
The recursive formula of filter gain value and error covariance matrix in H ∞ filter is used to replace and in SR-CKF algorithm, and the measurement noise is constantly updated to form SR-CKF algorithm based on H∞ filter. The flow chart is shown in the following Figure 4.
Compared with the traditional CKF algorithm, the SR-CH∞KF used the QR decomposition method to decompose and finishing iterating the error covariance matrix of the system state, which improves the stability of the algorithm. Furthermore, the H∞ filtering theory is introduced in the measurement updating part to update and modify the measurement noise continuously, which improves the filtering accuracy of the attitude measurement system.
5. Simulation and Result Analysis
To verify the performance of SR-CH∞KF in the linear-nonlinear system estimation problem, we adopt the way of simulation experiment, using the actual flight trajectory data of high-spinning aircraft as the theoretical truth value and using the theoretical truth value plus measurement noise as the measurement output value of Xc axis of geomagnetic sensor. Before the simulation experiment, it is necessary to know the longitude, latitude, and altitude of the simulation launch site. Taking a place in Chengdu as an example, the longitude is 30.67°, the latitude is 104.07°, and the altitude is 523.87 m. Based on the latest International Geomagnetic Reference Field (IGRF) given by the International Association of Geomagnetism and Aeronomy (IAGA), the geomagnetic parameters shown in Table 1 are obtained.
The high-spinning aircraft takes off by the external launcher, so it is necessary to determine the launching speed and attitude. Similarly, in the attitude measurement, the gyroscope also needs to input the initial attitude to calculate the subsequent attitude. Therefore, in the simulation experiment, it is also necessary to determine the initial conditions. The initial conditions of the simulation experiment are shown in Table 2.
To intuitively understand the filtering effect of SR-CH∞KF, we calculate the performance of four filtering algorithms, including unscented Kalman filter, cubature Kalman filter, square root cubature Kalman filter, and cubature Kalman filter based on singular value decomposition [22]. In Figure 5, the red curve in represents the pitch angle error change curve after UKF; blue represents the pitch angle error curve after passing through CKF; black represents the pitch angle error curve after SR-CKF; brown represents the pitch angle error curve after SVD-CKF; green represents the pitch angle error curve after SR-CH∞KF. After observing the pitch angle error range of the five kinds of filtering, we can know that the filtering effect of UKF is the worst among the five kinds of filtering. CKF and SVD-CKF show large errors in pitch angle estimation in the early stage, and SVD-CKF tends to be stable gradually in the later stage, but the variation of CKF error is still large. The error curves of SR-CKF and SR-CH∞KF are relatively stable, but the filtering accuracy of SR-CH∞KF about pitch angle is higher.
The curve color in Figure 6 represents the same filtering type as that in Figure 5. By observing the yaw angle error comparison diagram, it can be found that the filtering effect of CKF is the worst, and the filtering effect of UKF in the early stage is poor, but the error curve in the later stage is relatively smooth. The error curves of SVD-CKF and SR-CKF fluctuate greatly in the early stage and are relatively stable in the later stage. The filtering curve of SR-CH∞KF is more stable, and the filtering accuracy is higher than the other four kinds of filtering.
The filtering types represented by the curve colors in Figures 7 and 8 are the same as those in Figure 5. Within 600-800 s, due to the influence of the geomagnetic blind area, the error of roll angle is large. However, it can be seen from Figure 7 that SR-CH∞KF can effectively reduce the error of roll angle measurement in the geomagnetic blind area. In the nonblind area, it can be seen from Figure 8 that the filtering accuracy of SR-CH∞KF is relatively high. From the overall change of roll angle measurement error, we can know that SR-CH∞KF has higher filtering accuracy for roll angle. In the geomagnetic blind area, without the geomagnetic sensor, the three-axis gyroscope cannot measure the flight attitude of the aircraft independently. If the roll speed of the aircraft is low in the geomagnetic blind area, the three-axis gyroscope can measure the roll angle of the aircraft independently. In the future, we will study the independent attitude measurement of aircraft by gyroscope in geomagnetic blind area.
6. Conclusion
In this paper, an attitude measurement method of high-spinning aircraft based on SR-CH∞KF algorithm is proposed. This method combines H∞ filtering with SR-CKF algorithm and proposes a detailed filtering algorithm for linear-nonlinear systems. The difference is that the algorithm can be applied to the case of measurement side noise uncertainty, and the influence of measurement noise on the experimental results can be effectively reduced by continuously modifying the measurement noise by limiting the error parameters. The simulation results show that, compared with UKF, CKF, SR-CKF, and SVD-CKF, SR-CH∞KF algorithm can effectively improve the attitude calculation accuracy of high-spinning aircraft, which lays a good technical foundation for subsequent flight navigation and control. But the operation process of SR-CH∞KF algorithm is more complex than the other four algorithms. In the next step, we need to simplify it and propose a method that can effectively solve the error limiting parameters.
Data Availability
If you need further detailed data, please contact the corresponding author.
Conflicts of Interest
The authors declare that they have no conflicts of interest.