QMRPF-UKF Master-Slave Filtering for the Attitude Determination of Micro-Nano Satellites Using Gyro and Magnetometer

In this paper, the problem of estimating the attitude of a micro-nano satellite, obtaining geomagnetic field measurements via a three-axis magnetometer and obtaining angle rate via gyro, is considered. For this application, a QMRPF-UKF master-slave filtering method is proposed, which uses the QMRPF and UKF algorithms to estimate the rotation quaternion and the gyro bias parameters, respectively. The computational complexicity related to the particle filtering technique is eliminated by introducing a multiresolution approach that permits a significant reduction in the number of particles. This renders QMRPF-UKF master-slave filter computationally efficient and enables its implementation with a remarkably small number of particles. Simulation results by using QMRPF-UKF are given, which demonstrate the validity of the QMRPF-UKF nonlinear filter.


Introduction
Spacecraft attitude is important for attitude control in many space missions. In the last four decades, a great number of research works have been devoted to the problem of estimating spacecraft attitude based on a sequence of noisy vector observations, resolved in the body-fixed coordinate system and in a reference system [1][2][3][4][5][6][7].
For the attitude sensor, the sun sensor, Earth sensor, star tracker, three-axis magnetometer, and gyro etc, are often used. The attitude estimation problem possesses an undesirable strong nonlinearity. The filtering-based methods that were developed in the 1980s embedded the attitude determination problem in the framework of stochastic filtering. The mostly used filtering method is the extended Kalman filter (EKF) [2,8]. Nevertheless, poor performance or even divergence arising from the linearization implicit in the EKF has led to the development of other filters [3,9,10]. Another often used method is the unscented Kalman filter (UKF), also known as the sigma-point filter [4]. Unscented filters are essentially based on second or higher-order approximations of nonlinear functions, which are used to estimate the mean and covariance of the state vector. In [4], an unconstrained three-component vector is used, based on the generalized Rodrigues parameters to represent an attitude error quaternion. The state vector includes the attitude error and bias vectors. UKF, as a Kalman filter mechanization, is sensitive to the statistical distribution of the stochastic processes driving the dynamic model: non-Gaussian distributions guarantee nonoptimality of the estimates. Recently, a new method using the particle filtering (PF) technique has been proposed for a spacecraft's attitude estimation [1,[5][6][7]. Based on the concept of sequential importance sampling and the use of Bayesian theory, particle filtering is particularly useful in dealing with nonlinear and non-Gaussian problems. However, the most notorious disadvantage of a particle filter is its formidable computational complexity, since hundreds even thousands of particles are usually needed to achieve required approximation accuracy, and then it is difficult to achieve real time performance. A particle filtering algorithm [1] copes with the curse of computational complexity related to PFs by using an efficient initialization procedure, along with an importance weight cooling schedule and particle set portioning. This method is applicable to gyroless attitude determination setting, and the state vector is composed of a unit norm quaternion and a three-element angular-rate vector. In [5], particle filtering for attitude estimation using a minimal local-error representation (MLERPF) is proposed. The main character of MLERPF is that it does not work well when the number of particles is not large enough, which brings limitation to some applications.
This paper develops a QMRPF-UKF master-slave filter and gives its implementation in the case of a three-axis stabilized micro-nano satellite, obtaining noisy geomagnetic field measurements via a three-axis magnetometer and obtaining the noisy angle rate via gyro. In order to reduce the computational cost rising from the increased state dimensions, the quaternion multiresolution particle filter (QMRPF) algorithm is used as the master filter to estimate the attitude quaternion, and the unscented Kalman filter algorithm is used as the slave filter to determine the gyro bias parameters. On the other hand, the new estimator copes with the curse of computational complexity related to the particle filtering technique by introducing multiresolution approach that permits a significant reduction in the number of particles [11]. This renders the QMRPF-UKF master-slave filter computationally efficient and significant computational savings of QMRPF master filter is achieved by drastically reducing the needed number of particles. The performance of QMRPF-UKF in various scenarios is studied and compared with MLERPF method, which demonstrates the validity of QMRPF-UKF nonlinear filter. This paper is organized as follows. In Section 2, the system model of the attitude determination of a three-axis stabilized micro-nano satellite by using gyro and magnetometer is given. In Section 3, the QMRPF-UKF master-slave filter is described in detail. In Section 4, the QMRPF-UKF master-slave filter for the attitude determination problem is verified on the basis of its estimation accuracy and computational time. We draw some conclusions in Section 5.

Quaternion Process Model
It is known that the body angular motion can be described in terms of the attitude quaternion by the following equation: where [] k   denotes the cross-product matrix associated with the vector k  .

Rate Sensor Measurement Model
A common sensor that measures the angular rate is a rate-integrating gyro. For this sensor, a widely used model is given by: where k   is the measured rate; k  is the gyro bias vector; , vk  and , uk  are independent zero-mean Gaussian white-noise processes with: where E{· } denotes expectation and () t   is the Dirac_delta function.

Observation Model
A widely used attitude measurement model is given by: where k b is the body-frame vector and k r is the reference-frame vector; k b  denotes the measurement noise process, with known probability density function (PDF) denoted as k b denotes the rotation matrix that brings the axes of R onto the axes of B at time k . The attitude matrix is related to the quaternion by:

QMRPF-UKF Master-Slave Filter
In this Section, the QMRPF-UKF master-slave filtering method is proposed, which uses the QMRPF and UKF algorithms to estimate the rotation quaternion and the gyro bias parameters, respectively. Section 3.1 formulates a detailed process of QMRPF master-filter. Section 3.2 describes the UKF slave-filter process. The flow chart of the QMRPF-UKF master-slave filter with multiresolution approach embedded at time kM  is shown in Figure 1, where ① denotes the master filter, and ② denotes the slave filter.

QMRPF Master-Filter
QMRPF master-filter is provided to estimate the quaternion from pairs of vector observations, and computational efficiency of quaternion particle filter (QPF) is achieved by using the spatial-domain multiresolutional approach. This section is divided into three parts: QMRPF master-filter mathematical model, the complete step sum-up of QMRPF master-filter, quaternion particle filter with multiresolution embedded.

A. QMRPF Master-Filter Mathematical Model
Taking attitude quaternion as state variable, the mathematical model of QMRPF master-filter is built. State equation is Measurement equation is Equation (9). From Equation (11), it can be seen that, the gyro bias estimation at time 1 k  will be used for estimating the attitude quaternion at time k .

B. Complete Step Sum-up of QMRPF Master-filter
The time is supposed to be kM  when the multiresolution approach is embedded into the quaternion particle filter.
(1) Initializing the quaternion particles   0 1 N i i q  and setting quaternion weights as   (2) For kM  , quaternion particle filter is used to estimate the attitude quaternion ˆk q at time k , and the quaternion particles (3) For kM  , QMRPF algorithm is adopted to determinate the rotation quaternion ˆk q at time k , the number N of particles is reduced to be N  .
(4) For kM  , the basic step is the same as step 2, but the number of quaternion particles is N  .

C. Quaternion Particle Filter with Multiresolution Embedded
In the following, we will show how ˆk q is estimated when the multiresolution approach is embedded into the quaternion particle filter at time kM  .
(1) Particle weights multiresolution decomposition A partition of N samples into 2 L N blocks is used where L is the level number of multiresolution decomposition, and then particle weights of every block are decomposed into lowpassed and highpassed components.
where T denotes an orthogonal wavelet decomposition transform matrix; m denotes the m -th block; (2) Thresholding the highpass-filtered particle weights A simple thresholding is carried out on the highpass-filtered particle weights of every block: where s T is the wavelet threshold. Thus we have the thresholded weights: i mk q  , The number of quaternion particles is selected to be N  by the thresholding operation, which can significantly reduce the computation in propagation while maintaining performance. Some detailed information about the multiresolution process can be referenced to [11].
As in Davenport's well-known q-method, the quaternion that solves the maximization problem of Equation (18) is the normalized eigenvector corresponding to the largest eigenvalue of K : Then the attitude quaternion estimation ˆk q at time kM  is obtained.

UKF Slave-Filter
The gyro output can't be used for attitude estimation directly for the reason that the gyro has a bias vector, and thus the gyro bias is considered in the estimation process. The UKF slave-filter is designed to estimate the gyro bias vector. Taking gyro bias as state variable, the state space model of slave filter, UKF, is built on the basis of gyro bias model and vector observation model. State equation is Equation (6), and observation equation is: The noise variance matrix of state model and observation model are supposed to be 2 u Q  and b R  , respectively. It can be seen that, the state equation is linear and the observation equation is nonlinear. Moreover, the attitude quaternion at time 1 k  will be used to estimate the gyro bias at time k . The steps of gyro bias estimation using UKF is as follows: where 1k   and 1k P   denote the estimation value and estimation error variance matrix at time 1 k  , respectively; n denotes state dimension, and for this application, 3 n  . Supposing that the estimation (3) Calculating the weights of sigma points The weights corresponding to the sigma points are given by: (4) Time update The predicted mean and error variance matrix of the gyro bias at time k are given by: ˆk The prediction mean of the measurement vector at time k is calculated using a weighted sum of the points 1 () kk bi  , which is given by: The measurement variance and cross correlation between ˆk  and ˆk b are computed by: The Kalman gain is: Then the estimation value and estimation error variance matrix of gyro bias at time k are described by:

Simulation Results and Analysis
In our simulation, the micro-nano satellite orbit is in sun synchronization orbit with 900 km altitude. Gyro and magnetometer are integrated for attitude determination. The magnetic field reference is modeled using a 10th-order International Geomagnetic Reference Field model. Magnetometer noise is modeled by a zero-mean Gaussian white-noise process with a standard deviation of 50 nT. The gyros' output is contaminated with measurement noise having two components: white, zero-mean Gaussian process with intensity   . The starting time of performing multiresolutional transform is the 1,000 s. QMRPF-UKF master-slave filtering is used to perform the attitude determination of this micro-nano satellite. The codes are run on a computer, the processor speed of which is 2.5 GHz and memory is 2 Gbyte. In testing the filter, we look for two filter attributes: computational cost and accuracy.
The estimation error of gyro/magnetometer integrated attitude angle estimation is shown in Figure  2. In Figure 3, the gyro bias estimation error is given. It can be seen that the attitude angle estimation error is smaller than 0.1°, and the gyro bias estimation error is smaller than 0.01°/h. Here, we run QMRPF-UKF algorithm in 50 runs of Monte Carlo simulation with different thresholds to validate the effectiveness and complexity of QMRPF-UKF for the attitude determination of a micro-nano satellite. The performance is measured via RMS (root mean square) estimation error of Monte Carlo simulation. The smaller the RMS estimation error resulting from the filter is, the better the filter performance is. Figure 4 gives the selected particles numbers after performing multiresolution particle filter with different thresholds. RMS error and the computational cost of QMRPF-UKF with different thresholds are given in Table 1. It can be seen that, the larger the threshold is, the smaller the number of surviving particles is. With the increase of threshold, the computational time is decreased, and the filtering varies in estimation precision.
QMRPF-UKF is compared with the existing method, MLERPF, in terms of RMS error and computational cost, as shown in Table 1. It can be seen that, QMRPF-UKF outperforms MLERPF significantly in terms of computational cost and QMRPF-UKF is comparable to MLERPF in terms of estimation precision. For MLERPF, the state dimension of performing particle filter is six, which is larger than that of QMRPF-UKF. Moreover, in the calculating process of MLERPF, the particle number is invariant. For QMRPF-UKF master-slave filter, the filtering performance is improved from two aspects. On the one hand, in order to reduce the computational cost rising from the augment of the state dimensions, QMRPF algorithm is used as the master filter to estimate the attitude quaternion, and UKF is used as the slave filter to determine the gyro bias parameters. On the other hand, QMRPF copes with the curse of computational complexity related to the particle filtering technique by introducing multiresolution approach that permits a significant reduction in the number of particles.
Three simulation scenarios are carried out to verify the convergence performance of QMRPF-UKF. The first one is that, the initial attitude error angle is 50°, 50°, and 50° for each axis, respectively. The second one is that, the initial attitude error angle is −70°, 40°, and 100° for each axis, respectively. The third one is that, the initial attitude error angle is −60°, 30°, and 130° for each axis, respectively. These are added into the initial condition of attitude estimation. For each simulation scenario, other parameters are same as the above mentioned ones. QMRPF-UKF convergences in each simulation. The attitude angle estimation error is smaller than 0.1°, and the gyro bias estimation error is smaller than 0.01°/h.

Conclusions
We have developed a computationally efficient QMRPF-UKF master-slave filtering method, to solve the problem of gyro/magnetometer integrated attitude determination of a three-axis stabilized micro-nano satellite. Computational efficiency of the proposed filtering is achieved by combining the master-filter with slave-filter and by adopting the spatial-domain multiresolutional approach to reduce the needed number of particles while maintaining comparable estimation accuracy. The larger the threshold is, the fewer the number of surviving particles, and the filtering varies in estimation precision.