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 xorb, 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.
Sequential orbit and attitude determination scheme.
Satellite attitude kinematics equation can be described as
(1)q˙=12Ω(ωbo)q,
where q=[q1,q2,q3,q4]T represents the attitude quaternion from orbital coordinate system to body coordinate system and ωbo=[ωbo1,ωbo2,ωbo3]T represents the body angular velocity relative to orbital coordinate system. And Ω(ωbo) is written as
(2)Ω(ωbo)=[0ωbo3-ωbo2ωbo1-ωbo30ωbo1ωbo2ωbo2-ωbo10ωbo3-ωbo1-ωbo2-ωbo30].
The gyro measurement model usually takes the following form:
(3)u=ω+b+η1,
where u is the actual output value of gyro; ω is the body inertial angular velocity; b is the gyro constant drift; and η1 is the Gaussian white noise with the variance of σg2.
The body inertial angular velocity is calculated in (3). Consider
(4)ω=u-b-η1.
And, approximately,
(5)b˙=η2,
where η2 is drift slope white noise of gyro: η2~N(0,δb2).
In the satellite attitude kinematics equation, the body angular velocity relative to orbital coordinate system can be calculated by the following equation:
(6)ωbo=ω-C(q)ωo,
where C(q) is the attitude transformation matrix from orbital coordinate system to body coordinate system and ωo 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
(7)Bb=h(xorb,q)+vB=C(q)Bo(xorb)+vB,
where Bb represents the actual measured value of magnetometer and Bo(xorb) is the magnetic field vector in the orbital coordinate system, which can be calculated by IGRF. vB 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 system(8a)xk=Φk-1xk-1+wk-1,(8b)zk=Hkxk+vk,where xk represents n×1 state vector, zk represents m×1 observation vector, and Φk-1 and Hk are the state transition matrix and the observation matrix, respectively. wk and vk are the process noise and observation noise with average of 0 and variance of Q and R, respectively.
In the process of filtering, state one-step prediction can be expressed as
(9)x^k(-)=Φk-1x^k-1
and the covariance of one-step prediction error is
(10)Pk(-)=Φk-1Pk-1Φk-1T+Qk-1.
The prediction state residual vector and the measurement residual vector can be defined as (11), respectively
(11)vx^k(-)=x^k(+)-x^k(-)=x^k(+)-Φk-1x^k-1(+),vk=Hkx^k(+)-zk.
According to rule of robustness, build the following extreme function based on residual equation (11):
(12)Ωk=vkTR-k-1vk+αkvx^k(-)TPk-1(-)vx^k(-)-2λkT(Hkx^k(+)-zk-vk),
where R-k is the equivalent covariance matrix of zk, which is the adaptive estimate of measuring covariance matrix. αk (0<αk≤1) is the adaptive factor and λk is Lagrangian multiplier.
Take the partial respect of (12) to vk and x^k(+), respectively, then let them be equal to 0
(13)∂Ωk∂vk=2vkTR-k-1+2λkT=0,∂Ωk∂x^k=2vkTR-k-1Hk+2αkvx^k(-)TPk-1(-)-2λkTHk=0.
According to (13), it is can be deduced that
(14)2HkTR-k-1vk+αkPk-T(-)vx^k(-)=0.
Substituting (11) in (14), we obtain
(15)2HkTR-k-1(Hkx^k(+)-zk)+αkPk-T(-)(x^k(+)-x^k(-))=0.
From (15), we have
(16)x^k(+)=(HkTR-k-1Hk+αk2Pk-1(-))-1×(HkTR-k-1zk+αk2Pk-1(-)x^k(-)).
Let
(17)Kk=(HkTR-k-1Hk+αk2Pk-1(-))-1HkTR-k-1.
Then, (16) can be written as
(18)x^k(+)=Kk[zk+(Kk-1-Hk)x^k(-)].
Further, we can write (17) as
(19)Kk=1αkPk(-)HkT(1αkHkPk(-)HkT+R-k)-1.
So, the robust adaptive filtering algorithm for linear discrete system can be described as(20a)x^k(-)=Φk-1x^k-1,(20b)P-k(-)=1αk(Φk-1P-k-1(+)Φk-1T+Qk),(20c)K-k=P-k(-)HkT(HkP-k(-)HkT+R-k)-1,(20d)x^k(+)=K-k[zk+(K-k-1-Hk)x^k(-)]=x^k(-)+K-k(zk-Hkx^k(-)),(20e)P-k(+)=(I-K-kHk)P-k(-).
3.2. The Equivalent Covariance Matrix R-k of Observation Vector zk
From [24], the equivalent covariance matrix can be obtained from bifactor variance expanding model, whose element is
(21)σ-kij=λiλjσkij,
where σkij is the corresponding element of prior variance covariance and Rk, λi, and λj are variance inflation factors, which can be taken as the reciprocal of Huber weight function; that is,
(22)λii={1|v~i|=|viσvi|≤c,|v~i|c|v~i|≥c,
where c is constant and c=1.0~1.5, v~i 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 αk
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
(23)vinn,k=zk-Hkx^k(-)
and its theoretical covariance matrix is
(24)Pinn,k=HkPk(-)HkT+R-k.
It can be seen from (20a)–(20e), in robust adaptive filtering, innovation covariance is adjusted to
(25)P-inn,k=1αkHkPk(-)HkT+R-k.
Considering that P^inn,k represent actual innovation covariance matrix, the optimal adaptive factor αk based on innovation covariance satisfies
(26)P^inn,k=P-inn,k.
That is,
(27)P^inn,k=1αkHkPk(-)HkT+R-k.
From (27), we obtain
(28)αk(P^inn,k-R-k)=HkPk(-)HkT.
Substituting (24) in (28), we have
(29)αk(P^inn,k-R-k)=Pinn,k-R-k.
So, the optimal adaptive factor could be expressed as
(30)αk=tr(Pinn,k-R-k)tr(P^inn,k-R-k).
As the value range of adaptive factor αk is (0, 1],
(31)αk={1tr(Pinn,k)≥tr(P^inn,k)tr(Pinn,k-R-k)tr(P^inn,k-R-k)tr(Pinn,k)<tr(P^inn,k),
where tr(·) is trace of matrix. By omitting the same items in (31), an approximate optimal adaptive factor can be obtained
(32)αk={1tr(Pinn,k)≥tr(P^inn,k)tr(Pinn,k)tr(P^inn,k)tr(Pinn,k)<tr(P^inn,k).
Using the innovation vector of current sampling time to describe the real covariance, we have
(33)P^inn,k=vinn,kvinn,kT.
Consider
(34)tr(P^inn,k)=vinn,kTvinn,k.
The approximate optimal adaptive factor can be described as
(35)αk={1tr(Pinn,k)≥tr(P^inn,k)tr(Pinn,k)vinn,kTvinn,ktr(Pinn,k)<tr(P^inn,k).
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 λii=1, 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 αk=1, it degenerates into robust filtering. When λii=1 and αk=1, 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,(36a)x˙=f(x)+w,(36b)z=h(x)+v.
By Taylor series expansion and discretization, we obtain a robust adaptive filtering algorithm for nonlinear continuous system. Consider(37a)x^k(-)=∫(k-1)ΔtkΔtf(x^k-1(+))dt,(37b)P-k(-)=1αk(Φk-1P-k-1(+)Φk-1T+Qk),(37c)K-k=P-k(-)HkT(HkP-k(-)HkT+R-k)-1(37d)x^k(+)=x^k(-)+Kk(zk-h(x^k(-))),(37e)P-k(+)=(I-K-kHk)P-k(-),where Φk=I+(∂f(x)/∂x)|x=x^k(-)·Δt, Hk=(∂h(x)/∂x)|x=x^k(-), and Δt 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
(38)J=[18.10.4-0.480.416.3-4.61-0.48-4.6117.95]kg m2.
The initial position and velocity of satellite are
(39)r0=[5163062.03,4275992.16,-8.56]T m,v0=[-3288.1720,3964.9749,5263.4825]Tm/s.
The initial attitude quaternion and angular velocity are
(40)q0=[0,0,0,1]T,ω0=[0,0,0]Trad/s.
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:
(41)r^0=[5164062.03,4276992.16,1000]Tm,v^0=[-3188.1720,3864.9749,5163.4825]Tm/s.Q0=diag([1,1,1,1,1,1]*1e-8),P0=diag([1000,1000,1000,100,100,100]2).
Position estimation errors.
Orbital elements estimation errors.
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:
(42)q^0=[0.0523,-0.0474,0.0523,0.9961]T,b^0=[3.5,3.5,3.5]Tdeg/h,Q0=diag([1,1,1,1,1,1]*1e-10),P0=diag(-5,0.24e-5,0.24e-5].2[0.0474,0.0523,0.0474,0.24e-5,ssssssssss0.24e-5,0.24e-5]2).
Attitude estimation errors RSS.
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.
CôtéJ.De LafontaineJ.Magnetic-only orbit and attitude estimation using the Square-Root Unscented Kalman Filter: application to the PROBA-2 spacecraftProceedings of the AIAA Guidance, Navigation and Control Conference and ExhibitAugust 2008Honolulu, Hawaii, USA2-s2.0-78651105855PsiakiM. L.HuangL.FoxS. M.Ground tests of magnetometer-based autonomous navigation (MAGNAV) for low-earth-orbiting spacecraft19931612062142-s2.0-0027307993WiegandM.Autonomous satellite navigation via Kalman filtering of magnetometer data1996384-8395403JungH.PsiakiM. L.Tests of magnetometer/sun-sensor orbit determination using flight data20022535825902-s2.0-0036564249ThienelJ. K.HarmanR. R.Bar-ItzhackI. Y.LambertsonM.Results of the magnetometer navigation (MAGNAV) inflight experimentProceedings of the AIAA/AAS Astrodynamics Specialist Conference and ExhibitAugust 2004Providence, RI, USA2062222-s2.0-19644390938TortoraP.OshmanY.SantoniF.Spacecraft angular rate estimation from magnetometer data only using an analytic predictor20042733653732-s2.0-2642571018DingW.WangJ.RizosC.KinlysideD.Improving adaptive kalman estimation in GPS/INS integration20076035175292-s2.0-3774900442010.1017/S0373463307004316GaoS.ZhongY.LiW.Robust adaptive filtering method for SINS/SAR integrated navigation system20111564254302-s2.0-8005226016610.1016/j.ast.2010.08.007ZhangH.ShiY.WangJ.On energy-to-peak filtering for nonuniformly sampled nonlinear systems: a Markovian jump system approach2013221212222ZhangH.ShiY.Saadat MehrA.Robust energy-to-peak filtering for networked systems with time-varying delays and randomly missing data20104122921293610.1049/iet-cta.2009.0243MR2808632ZhangH.MehrA. S.ShiY.Improved robust energy-to-peak filtering for uncertain linear systems2010909266726752-s2.0-7795527272310.1016/j.sigpro.2010.03.011ZhangH.ShiY.Parameter-dependent H∞ filtering for linear time-varying systems20131352021006GaoH.LamJ.WangC.Mixed H2/H∞ filtering for continuous-time polytopic systems: a parameter-dependent approach200524668970210.1007/s00034-005-0612-yMR2200925ZBL1102.94033ZhangH.ShiY.Saadat MehrA.On H∞ filtering for discrete-time takagi-sugeno fuzzy systems20122023964012-s2.0-8485970908210.1109/TFUZZ.2011.2175933ZareiJ.TajeddiniM. A.KarimiH. R.Vibration analysis for bearing fault detection and classification using an intelligent filter201424151157WeiY.QiuJ.Reza KarimiH.A new design of H∞ filtering for continuous-time Markovian jump systems with time-varying delay and partially accessible mode information201393923922407BasinM.MaldonadoJ. J.KarimiH. R.Mean-square filtering for polynomial system states confused with poisson noises over polynomial observations201132247552-s2.0-8005187022610.4173/mic.2011.2.1KarimiH. R.Robust delay-dependent H∞ control of uncertain time-delay systems with mixed neutral, discrete, and distributed time-delays and Markovian switching parameters20115881910192310.1109/TCSI.2011.2106090MR2857623SokenH. E.HajiyevC.Pico satellite attitude estimation via Robust Unscented Kalman Filter in the presence of measurement faults20104932492562-s2.0-7795565306010.1016/j.isatra.2010.04.001HideC.MooreT.SmithM.Adaptive Kalman filtering algorithms for integrating GPS and low cost INSProceedings of the Position Location and Navigation Symposium (PLANS '04)April 20042272332-s2.0-2942639707HajiyevC.Adaptive filtration algorithm with the filter gain correction applied to integrated INS/radar altimeter200722158478552-s2.0-3674900509910.1243/09544100JAERO173DouglasS. C.MadisettiV. K.Introduction to adaptive filters2010Boca Raton, Fla, USATaylor & Francis118KimK. H.LeeJ. G.ParkC. G.Adaptive two-stage Kalman filter in the presence of unknown random bias200620730531910.1002/acs.900MR2252244ZBL1130.93052YangY.SongL.XuT.CarosioA.KuttererH.New robust estimator for the adjustment of correlated GPS networksProceedings of the 1st International Symposium on Robust Statistics and Fuzzy Techniques in Geodesy and GIS2001IGP-Bericht199208No. 195