Abstract
Based on magnetometer and gyro measurement, a sequential scheme is proposed to determine the orbit and attitude of small satellite simultaneously. In order to reduce the impact of orbital errors on attitude estimation, a robust adaptive Kalman filter is developed. It uses a scale factor and an adaptive factor, which are constructed by Huber function and innovation sequence, respectively, to adjust the covariance matrix of system state and observational noise, change the weights of predicted and measured parameters, get suitable Kalman filter gain and approximate optimal filtering results. Numerical simulations are carried out and the proposed filter is approved to be robust for the noise disturbance and parameter uncertainty and can provide higher accuracy attitude estimation.
1. Introduction
Small satellite, as an integration of new concept, new theory, and new method, has been widely used in the fields of remote sensing, communication, astronomical observation, and new technology experiment. Restricted by the size and weight of small satellite, microsensors are required to be used in its navigation system. Magnetometer is widely used in the satellite because of its characteristics of low cost and high reliability [1]. Especially micromagnetometer with new technologies such as microelectronics and micromechanical can be made as light as dozens of grams. And its measuring information contains both the orbit and attitude information. For the above reasons, micromagnetometer became the first choice of the small satellite navigation sensors. But because of the low accuracy of magnetic field measurement, satellite navigation precision, especially the attitude angular velocity estimation precision, is not high. So, the magnetometer and gyro combination can significantly improve the navigation accuracy.
In 1993, Psiaki put forward the thought of the satellite navigation by using the geomagnetic field [2]. Then, many scholars made a lot of work on it and have made a lot of research achievements [3–5]. In current research, satellite orbit and attitude are mostly considered as two independent systems when determining them and tandem orbit and attitude determination strategy are taken. In the orbital state estimation stage, to avoid the influence of attitude errors, the modulus of geomagnetic field vector is taken as observed quantity. And in the attitude determination stage, orbital parameters are considered to be known; attitude optimal estimation is obtained by using the standard Kalman filtering algorithm, regardless of the influence of orbit determination errors [6].
However, in the attitude determination stage, both state equation and observation equation of system are functions of orbit parameters. On this occasion, the errors of orbit parameters given by orbit filter are bigger, and especially when the system has not been convergent, the traditional Kalman filter will not be able to obtain the optimal results of attitude determination. It is necessary to put forward a kind of filtering algorithm to realize high precision attitude determination considering the effects of orbit errors.
Extended Kalman filtering (EKF) algorithm has been widely used in the nonlinear estimation problem such as satellite attitude determination. The estimation performance of EKF is not only associated with the model precision of system but also related to the statistical characteristic of noise. But in practice, due to the complex surroundings, the characteristics of the noise are very difficult to get accurately [7]. Traditional EKF does not have the adjust ability for noise and is not sensitive to uncertainty of model parameters, which directly affects the estimation performance of EKF. In order to solve these problems, the traditional EKF needs to be improved. A feasible method is to use adaptive Kalman filtering. It can estimate online the statistical characteristic of the noise in real time, so as to adjust the filter parameter [8]. Many results on estimation and adaptive filtering design for different kinds of dynamic systems have been obtained [9–18]. Adaptive filtering can be divided into three basic types: multimode self-adaptive filtering, self-adaptive filtering based on the innovation, and adaptive filtering based on the residual [19]. Multimode adaptive filtering requires multiple filters; they are calculated in parallel according to the different model. This method is not only complex but also with heavy computing burdens [20]. In the self-adaptive filter based on innovation or residual, covariance matrix could adjust adaptively according to the innovation or residual sequence through fuzzy logic control. But the fuzzy rules are often established based on people’s experience [21, 22]. The other feasible method is adaptive fading Kalman filter. It uses a suboptimal fading factor to enhance the impact of innovation sequence, but filtering accuracy will be reduced [23].
For the attitude determination problem considered in this paper, there is still some limitations by using the above improved EKF method singly. So, on the basis of the above research, a robust adaptive Kalman filtering has been raised in this paper. It combines the robust filtering methods and adaptive filtering. By adjusting the observation noise and the system covariance matrix, it changes the weights of original data and observation information in the filter and then gets more reasonable filter gain and effectively improves attitude determination accuracy, which provide a feasible way for small satellite autonomous navigation problem based on low-cost sensors.
2. Problem Description
For the satellite orbit and attitude determination problem based on magnetometer and gyro, a sequential scheme is adopted, in which the determination of orbit and attitude are considered as two independent stages. In the stage of estimating orbit states , in order to avoid the influence of satellite attitude, EKF is taken to determine the orbit parameters, by taking the modulus of geomagnetic field vector measured by magnetometer as observation. In attitude determination stage, filtering algorithm is adopted to estimate attitude using the estimated orbital parameters and geomagnetic field vector, as well as gyro measurement; see Figure 1. Orbit estimation stage will not be studied in this paper because there have been a lot of research results about it; see [3]. By the reason of taking the sequential orbit and attitude determination scheme, the attitude determination is not only affected by the precision of sensor precision but also affected by the precision of orbit estimation. Focusing on the attitude determination stage, a satellite attitude determination algorithm, which can inhibit the influence of orbit errors, is put forward.
Satellite attitude kinematics equation can be described as where represents the attitude quaternion from orbital coordinate system to body coordinate system and represents the body angular velocity relative to orbital coordinate system. And is written as
The gyro measurement model usually takes the following form: where is the actual output value of gyro; is the body inertial angular velocity; is the gyro constant drift; and is the Gaussian white noise with the variance of .
The body inertial angular velocity is calculated in (3). Consider
And, approximately, where is drift slope white noise of gyro: .
In the satellite attitude kinematics equation, the body angular velocity relative to orbital coordinate system can be calculated by the following equation: where is the attitude transformation matrix from orbital coordinate system to body coordinate system and represents the orbit angular velocity, which can be obtained from orbit parameters.
When determining the attitude, taking three-axis magnetic field vector measured by magnetometer as observation, the observation equation is where represents the actual measured value of magnetometer and is the magnetic field vector in the orbital coordinate system, which can be calculated by IGRF. represents magnetometer measurement errors, which is Gaussian white noise.
It is can be seen from (6) and (7) that both the system state equation and observation equation are functions of orbit parameters in the process of attitude determination. But the real value of orbit parameters cannot be gotten, so we can only use the result of orbit filter to replace them. So, additional errors were introduced for the system. Therefore, the traditional Kalman filter will not be able to obtain the optimal results of altitude determination. The errors that occurred in this part can be considered as parameter uncertainty. The purpose of this paper is to design a robust adaptive filter to cope with the attitude determination problem caused by the uncertainty of orbital parameters and improve the accuracy of attitude determination.
3. Robust Adaptive Filtering Algorithm for Attitude Determination
Satellite attitude determination system is a continuous nonlinear system, and it should be linearized and discretized for application of EKF filtering. For convenience of derivation, robust adaptive filter for linear discrete system is firstly considered in this section, and then it is extended to nonlinear continuous system.
3.1. The Design of Robust Adaptive Filter
Consider the linear discrete systemwhere represents state vector, represents observation vector, and and are the state transition matrix and the observation matrix, respectively. and are the process noise and observation noise with average of 0 and variance of and , respectively.
In the process of filtering, state one-step prediction can be expressed as and the covariance of one-step prediction error is
The prediction state residual vector and the measurement residual vector can be defined as (11), respectively
According to rule of robustness, build the following extreme function based on residual equation (11): where is the equivalent covariance matrix of , which is the adaptive estimate of measuring covariance matrix. () is the adaptive factor and is Lagrangian multiplier.
Take the partial respect of (12) to and , respectively, then let them be equal to 0
According to (13), it is can be deduced that
Substituting (11) in (14), we obtain
From (15), we have
Let
Then, (16) can be written as
Further, we can write (17) as
So, the robust adaptive filtering algorithm for linear discrete system can be described as
3.2. The Equivalent Covariance Matrix of Observation Vector
From [24], the equivalent covariance matrix can be obtained from bifactor variance expanding model, whose element is where is the corresponding element of prior variance covariance and , , and are variance inflation factors, which can be taken as the reciprocal of Huber weight function; that is, where is constant and , is standardized residual. Therefore, the observation covariance matrix inflates when the observed error exceeds a certain range. Otherwise, the prior variance remains unchanged.
3.3. The Selection of Adaptive Factor
Because the innovation vector can reflect the errors of the system, we consider solving the adaptive factor by using innovation.
In the process of filtering, the innovation sequence can be described as and its theoretical covariance matrix is
It can be seen from (20a)–(20e), in robust adaptive filtering, innovation covariance is adjusted to
Considering that represent actual innovation covariance matrix, the optimal adaptive factor based on innovation covariance satisfies
That is,
From (27), we obtain
Substituting (24) in (28), we have
So, the optimal adaptive factor could be expressed as
As the value range of adaptive factor is (0, 1], where is trace of matrix. By omitting the same items in (31), an approximate optimal adaptive factor can be obtained
Using the innovation vector of current sampling time to describe the real covariance, we have
Consider
The approximate optimal adaptive factor can be described as
Compared with the standard EKF, the robust adaptive filtering only adds a judgment process of two scalar factors in the loop of each step, the complexity and calculated quantity of which will not be increased substantially. When the variance inflation factor , this means that the measurement noise is the same as the original statistical characteristic, the robust adaptive filtering degenerate into adaptive filtering. When the adaptive factor , it degenerates into robust filtering. When and , it is the traditional Kalman filtering. So, the robust adaptive filtering is still Kalman filtering substantially, which is capable of adapting to the influence of uncertain factors by adjusting the parameters properly and can realize the states estimate in the condition of uncertain factors.
For nonlinear continuous system,
By Taylor series expansion and discretization, we obtain a robust adaptive filtering algorithm for nonlinear continuous system. Considerwhere , , and are sampling time steps.
4. Mathematical Simulation and Analysis
In this section, the performance of the robust adaptive filtering is verified by mathematical simulation.
In the simulation, the real parameters of microsatellite are given as below.
The moment of inertia of satellite is
The initial position and velocity of satellite are
The initial attitude quaternion and angular velocity are
The measurement accuracy of magnetometer is 100 nT, the constant drift of gyro is 3 deg/h, the measurement accuracy of it is 0.1 rad/h, the sampling period is 1 s, and the simulation time is 10000 s.
For the determination of orbit, taking the earth magnetic field as measurement information and using the standard EKF algorithm for estimation, the initial simulation parameters are set as follows and the simulation results are shown in Figure 2 to Figure 3:
(a)
(b)
(c)
(d)
After determining the satellite orbit parameters, taking the estimated results as the input of the attitude determine system, attitude robust adaptive filtering algorithm is adopted to determine the attitude and simulation initial parameters as follows and the simulation results are shown in Figure 4:
Figure 2 shows the three-axis position estimation errors by taking the magnitude of the geomagnetic field as observation; Figure 3 shows the estimation errors of corresponding orbital elements. As seen from the figures, when the position error is 1000 meters, the effect of the errors cannot be ignored for both attitude kinematics equation and the observation equation. Figure 4 shows the attitude estimation errors based on traditional EKF and robust adaptive filter. It is can be seen from Figure 4. The estimation precision of the traditional EKF is about 0.2 degrees, and the maximum estimate error of the robust adaptive filter proposed in this paper is 0.1 degrees; the accuracy has improved significantly. Furthermore, the oscillation of robust adaptive filter estimation results is smaller, and the curve is smoother; it also illustrates that the robust adaptive filter has stronger ability to adapt to the interference or uncertainty.
5. Conclusions
In this paper, a robust adaptive Kalman filtering algorithm is proposed, aiming to investigate the influence of the orbit determination errors on the attitude determination. This algorithm not only adaptively adjusts the covariance of the observation noise, but also adjusts the covariance matrix of one-step prediction using the adaptive factor, which changes the weight of old data and observation information in the filter, getting a more appropriate Kalman gain. The noise variance inflation factor based on Huber weight function and adaptive factor based on the innovation proposed in this paper ensure the approximate optimal of filtering results. By taking attitude determination of small satellite with magnetometer and gyro combination as an example, a series of mathematical simulations are carried out. The results show that the proposed robust adaptive filtering algorithm can better cope with the interference of noise and uncertainty of system model parameter which provide a higher accuracy of attitude determination results under the premise of errors existing in orbit parameters determination.
Conflict of Interests
The authors declare that there is no conflict of interests regarding the publication of this paper.