spacecraft attitude estimation using adaptive gaussian sum filter
TRANSCRIPT
The Journal 01 the Astronautical Sciences, Vol. 57, Nos. 1 & 2, January-June 2009, pp. 31-45
Spacecraft Attitude Estimation Using Adaptive
Gaussian Sum Filter1
Jemin George/ Gabriel Terejanu/ and Puneet Singla4
Abstract
This paper is concerned with improving the att itude estimation accuracy by implementing an adaptive Gaussian sum filter where the a posteriori density func tion is approximated by a sum of Gaussian density functions. Compared to the traditional Gaussian sum fi lter, this adaptive approoch utilizes the Fokker-Planck-Kolmogorov residllal minimization to update the weights associated with different components of the Gaussian mixture model. Updating the weights provides an accurate approx imation of the a (JOste riori density fu nction and thus superior estimates. Simulation results show that updating the weights during the propagation stage not only provides better estimates between the observations but also provides superior estimator perfonnance where the measurements are ambiguous.
Introduction
The spacecraft attitude estimation problem involves detem1ining the orientation of a spacecraft from on-board observations of line-oF-sight vectors to various reference points such as celestial bodies, the direction of the Earth 's magnetic fie ld gradient, e tc . [ 11 . Generally, a redundant set of these observations is used to generate more accurate estimates of the spacecraft attitude. Several attitude sensors are discussed in the literature, including three axis magnetometers, Sun sensors, Earthhorizon sensors, globa l positioning sensors, rale integrating sensors and star-cameras [2]. Accuracy of the estimated attitude depends on the quality of atti tude sensors used. The estimation algorithm is required to extract the useful information from the available sensor measurements. wh ich are often corrupted by sensor noise, biases and sensor inaccuracies. Generally. the attitude estimation algorithms can be divided into two categories: I) batch atti tude estimation algorithms
'Presented at the F. t...andis Markley Astronautics Symposium. Cambridge. Marylaml. June 29-July 2. 2008. 'Graduate Student. Department of Mechanical & Aerospace Engineering. University at Buff~lo. Buffalo. NY-14260. E-mail: [email protected]. 'Gradua1e SlUdem. Depanmem of Computer Science & Engineering. Universi1y at Buffalo. Buffalo. NY· 14260. E-mail: [email protected]. ' Assistant Professor. Departl11ent of Mechanical & Aerospace EngillCering. University at Buffalo. Buffalo. NY· 14260. E·mail : [email protected].
31
32 George, Terejanu, and Singia
such as TRIAD [3], QUEST [41, ESOQ [5, 6], etc and 2) sequential altitude esti~ malion algorithms such as Kalman filter L71. REQUEST L8], unscented Kalman filter [91, particle filter [10], etc. A detailed discussion on various attitude estimation algorithms can be found in reference [Ill. During normal operations, the attitude estimation problem needs to be solved recursively; i.e. , the attitude fi lter makes new updates and predictions based on present and prior sensor information. Though many recursive attitude estimation algorithms are presented in the literature, the Extended Kalman Filter (EKF) is the most widely used nonlinear estimator. It is based on the assumption that the nonlinear attitude dynamics can be accurately modeled by a fi rst-order Taylor series expansion [7].
Since the EKF onl y provides a rough approximation of the a posteriori probabili ty density function (pdt) and solving for the exact solution of the a posteriori pdf is often impossible , researchers have been looking for mathematically convenient approxi mations. The accuracy and the efficient implementat ion of estimators based on these approximations have been issues . The Bayes filter [12] offers the optimal recursive solution to the nonlinear estimation problem. However, the implementation of the Bayes filter in real time is computationally intractable because of the multidimensional integrals involved in the recursive equations. An efficient implementation of the Bayes filter is possible when all the involved random vari ables are assumed to be Gaussian [1 3- 14J. Several other approximate techniques such as Sequenti al Monte Carlo (SMC) methods [ 15], Gaussian closure [16] (or higher order moment closure), equivalent linearization [17], and stochastic averaging r 18, 191 can also be used to find a solution to non linear estimation problem. SMC methods or particle filters [20] consist of discretizing the domain of the random variable into a set of finite number of particles and transforming these particles Ihrough the nonlinear map to obtain the distribution characteristics of the transformed random variable. Although SMC based methods are suitable for highly nonlinear/non-Gaussian models and have been successfully applied for the attitude esti mation problem [10] , they require extensive computational resources and effort [21]. Alternative to sequential Monte Carlo method is the exact non linear filters based upon the exact solution of the Fokker-Planck-Kolmogorov (FPK) equation [22] as discussed in references L21, 23J. The FPK equation provides the exact description fo r the propagation of the state transition pdf through a nonlinear dynamical system. Analytical solutions ex ist only for stationary pdf and are restricted to a limited class of dynamical systems [22, 24]. Thus researchers are looking active ly at numerical approximations to solve the FPK equation [25-28], generally using the variational formulation of the problem. However, these methods are severe ly handicapped for higher dimensions and have not found much use in the development of exact nonlinear fi lle rs . In reference [29], an approximate solution to the FPK equation is presented for the att itude est imation problem.
The main idea of this paper is to develop a nonlinear filter called the adaptive Gaussian sum filter for the spacecraft attitude estimation problem. In a tradit ional Gaussian Sum Filter (GSF), the a posteriori pdf is approximated by a weighted sum of Gaussian density fu nctions and an EKF is used to propagate the mean and covariance associated with each Gaussian density function. It can be shown that as the number of Gaussian density functions increases, the Gaussian sum approx imalion converges uniform ly to any probability density funct ion [30]. When the observations are available, both the moments and the weights are accordingly updated [31,32] using Bayes rule to obtain an approximation of the a posteriori pdf.
Spacecraft Anltude Estimation Using Adaptive Gaussian Sum Filter 33
The weights associated with the individual Gaussian densities are selected based on the measurement residual likelihood function and are kept constant during the propagation stage . Though extensive research has been done on GSF [20], in most of the nonlinear est imation approaches based on the Gaussian sum approximation, the weights of different components of a Gaussian mixture are kept constant while propagating the pdf through a linearized model and are updated ollly in the presence of measurement data. This assumption is valid if the underlying dynamics is linear or the system is marginally nonlinear or the measurements are precise and are available very frequently. The same is not true for the general non linear case. In our prior work [33~35J, an adaptive GSF is developed by adapting the weights of the Gaussian mixture model during both the propagation and measurement update stages . In the adaptive GSF, the weights are updated during the propagation stage so that the FPK residual is minimized. In other words, the weights of the individual Gaussian pdfs are selected so that the error between the time derivative of the Gaussian sum mixture and the FPK equation is minimized. The present paper is concerned with utilizing this recently developed approach for the attitude estimation problem.
The struct ure of this paper is as follows. First, a brief review of the alt itude sensors and atti tude kinematics is presented. Next , an introduction to the convent ional Gaussian sum approximation is presented followed by a novel scheme for updming the weights of different components of Gaussian mixture based on the FPK residual mi nimization . Finally, numerical examples are considered to illustrate the efficacy of the proposed method.
Attitude Sensors and Dynamic Models
In this section, a brief review of the attitude sensors and attitude kinematic relations is presented. An attitude sensor is an instrument which measures the orientation of a reference vector, such as a unit vector directed towards a known star, in the spacecraft body frame of reference. By determining the orienlation of two or more of these reference vectors relative to the spacecraft axes , the spacecraft attitude can be determined . The attitude kinematics defines the relation between the temporal derivative of the altitude representation and the angular velocity.
Attitude Sensor Model
We assume that the spacecraft attitude is determined by processing the digital images of the stars by a star-camera. Star positions are the most accurate source of reference celestial bodies for the attitude detennination since their posit ion with respect to an inert ial frame is fixed. Pixel formats of the order of 5 12 x 512 or larger are commonly used to provide good resolution pictures. The first stage in attitude determination is to identify a star with reference to an on-board star catalog. After star identification is made, image plane coordinates of the stars are given by using a pinhole camera model for the camera . Photograph image plane coordinates of the jIll star are given by the ideal co-l inearity equations
. Cl l r~j + CnrYj + C3r~j x'~ f +",
C 13rXj + Cn rYj + C33r~j (I)
. C21rXj + Cn rYj + C23r~j y'~ f +Yo
C UrXj + C32r)"j + C33r :j (2)
34 George, Terejanu, and Slngl8
wherefis the effective focal length of the star-camera and (xo, Yo) is the princ ipal point offset, determined by the ground or on-orbit calibration [36]. The terms Cij arc the attitude matrix elements (orienting the sensor frame relat ive to the inertial axes). and the inertial star vector r j is given by
{"'} {cos 8) cos a,}
r j = r" = cos ~j sin U j
rq Sin 5j
(3)
Further, choosing the z-ax is of the image coordinate system towards the boresight of the star-camera the measurement vector bi is given by the expression
. 1 {-(X' - .to)} b' = vi 2 2 / 2 - (Yi - Yo)
Xj+Yj+ f (4)
The relationship between the measured star direction vector bi in image space and its projection r j on the inertial frame is given by
bj= Cr'+ v1 (5)
where'; is a zero-mean Gaussian white noise sequence with covariance matrix Ri. The attitude determination problem reduces to the estimation of the elements of the attitude matrix C given the well calibrated image plane star-vectors , r j and corresponding inertial plane vector b j
. The spacecraft attitude can be represented by many coordi nate choices [37.38], but in this paper Modified Rodrigues Parameters (MRP). p E SO(3), are used. The parametrization of the atti tude matrix in tenns of the MRP, C(p), is defined as
4(1 - p' p) 8 C(p) ~ I", - ( 1 + p' p)' [p X] + ( 1 + p' p)' [p x] ' (6)
where [px ] is a 3 X 3 skew-symmetric cross product matrix given as
[p x] ~ [ : , -;' ~;, ] (7)
- P2 PI 0 Gyro Model
The spacecraft attitude is nonnally estimated by a combination of the attitude sensor (e .g. star tracker) measurements along with a model of spacecraft dynamics. The use of densely available angu lar rate data can omit the need or the spacecraft dynamic model. Rale gyros are used to measure the angular rates or the spacecraft without regard 10 the a ttitude of the spacecraft. Generally, rate gyro measu rements are modeled by the expression [2,7]
w(1) ~ W(I) - b(l) - " ,(I) (8)
where w(t) E RJ is the unknown true angular ve locity of the spacecraft, w (t) E RJ
is the gyro measured angul ar rale of the spacecraft in the gyro frame , and b (t) E RJ
is the gyro bias drift vector, which is further modeled by the first-order stochastic process
b (I ) ~ " ,(1) (9)
Spacecraft Attitude Estimation Using Adaptive Gaussian Sum Filter 35
where 71 1(1) and 71 2(1) are assumed to be modeled by two independent Gaussian white noise processes with
E ([ ~;i;~ ]r ~i(1 + T) ~1(1 + T)]} ~ QS( T)
The matrix Q is assumed to be diagonal, i.e.
Q = diag[ql q2 q3 q4 qs q6]
Alli/ude Kinematics
The kinematic equations for the spacecraft motion using the MRP as attitude parameters can be written as
B(p) ~ [(I - pTp)lw + 2[px] + 2ppT] (IDa)
( lOb)
Among the different parameter descriptions of attitude, only the MRPs have the remarkable property that the kinematic coefficient matrix, B(p) , has orthogonal rows and columns . Generally, these parameters are well behaved for most large motions exclusive of tumbling motions since a full revolution of the rigid-body about any axis is required to encounter the singularity. Their si ngularity-free state space volume ( ::!:: 27T about any axis) is substantially larger than that of any Euler angle representation (never farther than 7T/2 from a singularity) and the classical Rodrigues parameter representat ion ( ::!:: 7T about any axis). Furthennore, in reference {381 it is shown that the MRPs are the most nearly linear three-parameter representation of attitude.
Estimator Design
In this section a detailed fonnulation of the adaptive Gaussian sum filter for spacecraft altitude estimation is presented. The state vector of the adaptive Gaussian sum filter consists of MRP, p and the gyro bias, b, as
{p(t)}
X(/) ~ b(/) (II)
We mention that in this case, equation (IDa) along with equations (8) and (9) constitute the assumed dynamic model given by the equation
x(tl ~ f(x(tl, ';; (/)) + g(X(/))r(/), X(IO) = Xu (12)
where
f(X(/) , ';; (/)) ~ [B(P)(';; - b)], 0 3X I
g(X(/» ~ [ - B(P) 0,,,], r(/) ~ [~' (/) ] 03)( 3 h X3 71ll)
Further, equation (5) constitutes the measurement model for the attitude estimation problem such that
Yt = h(XJ,) + Vt
where Yl is a vector of star directions in image frame and therefore
h(Xt) = C(pt)r j
( 13)
36 George, Terejanu, and Singla
The measurement noi se Yk is assumed to be a zero-mean Gaussian white-noise sequence with E[VkVJ] = RkSkj.
COllventional Gaussiall Slim Filler
In th is section , we briefly discuss the development of the conventional Gaussian sum filter. More details about the development of a conventional Gaussian sum fi lter for a generic nonlinear dynamical system can be found in references [1 3, 31]. Now, we consider the six-dimensional continuous-time disturbance driven nonlinear dynamic system and discrete measurement model given in equations (12) and (13), respectively. The development of the conventional Gaussian sum filter fol lows from the assumption that the underlying conditional pdf (0 priori pdf jf I > k or a posleriori pdf if t = k) can be approx imated by a finite sum of Gaussian pdfs
N
p(x, I Yl) = 2: W;llPgi (14) i - I
where the individual Gaussian components are given as
PJi = N(x, - 1t ~1.'P~k) = 1 21T~kl - '/2exp [ - ~ (x, - 1t~)Tp;f1(X , - 1t~1t ) ] where Yk denotes a set of k observations, Yl = {Yil i = I .. . k}, and w.il[.t denotes the weight associated with the illl Gaussian in the mixture. Finally, It :lk and P/i.t represent the conditional mean and covariance of the itll component of the Gaussian pdf, respectively. The positivity and nonnalization constraint Oil the mixture pdf, p(x,IYk), leads 10 constraints on the weights as
N
L:w~ = I, wilt > 0, 'rIto:O=:; k:O=:;t (15) i - I
Since all the components of the mixture pdf of equation (14) are assumed to be Gaussian, only their mean and covariance need to be propagated between the sampli ng interval, [k , k + I], using the conventional Extended Kalman Filter lime update equat ions [31]
,i;(I) ~ f(,,;(I), .0(1))
hI) ~ A'(IW(I) + p '(M' (I) + g(,,;(t))Qg'(,,;(I))
A'(I) ~ . f (' (I), .0 (1))1 ox(t) xlr)_"i(ll
Notice that the weights are fixed during the propagation stage, i.e .
( 16)
( 17)
( 18)
w~=wilt for k :O=:; t S; k + I (19)
If we partition the itll mean vector, Iti(t) = HWt),bi(tW, then the Jacobian matri x Ai(t) correspondi ng to attitude estimation problem is given as
A;(I) ~ [ F(" ;(I)) - 8 (,,;(1)) ] (20) O ]X] O ]X]
where B(') is given in equation ( lOa) and F(·) is given as
Spacecraft Attitude Estimation Using Adaptive Gaussian Sum Filter 37
Similarly, both the state vector and the covariance matrix can be updated using the extended Kalman Filter measurement update equations and further the weights of different Gaussian mixture components can be updated using Bayes rule but under the assumption that P:lk ---)0 0 as is shown in reference [13]
where
lL~k = 1L~,k-I + K1{Y. - h (Il~~- I )}
p ko) = {hX6 - KtH1}P'ijt -J
H' _ Uh(X')1 k - ~ Xk- ,.:it- I
KJ = P~_ I (H1)T{H;E1IJ; _ J(mY + Rkr l
(22)
(23)
(24)
(25)
(26)
(27)
Finally, an optimal state estimate and corresponding error covariance matrix can be obtained by making use of the relations
(28) i - I
N
p" ~ I w~. [p~ + (I'~ - I'~)(I':' - 1',,)' ] (29) i- I
Notice that in traditional GSF it is assumed that weights w~ do not change between measurement updates. This assumption is val id if the underlying dynamics is linear. The same is not true for the general nonlinear case and new estimates of weights are required for accurate propagation of the state pdf. It is assumed that the covariance of various Gaussian components are small enough (31] such that the linearizations become representative for the dynamics around the means. This is a problem particularly if the uncertainty in the measurement model is large relative to the process noise or measurements are not ~vailab le frequently. In practice this assumption can be easily violated resulting in a poor approximation of the forecast pdf. Practically, the dynamic system may exhibit strong nonlinearities and the total number of Gaussian components required to accurately approximate the pdf may be restricted due to computat ional requirements. The existing literature provides no means for adaplion of the weights of different Gaussian components in the mixture model during the propagation of the state pdf. The lack of adaptive means for updating the weights of Gaussian mixture are felt to be serious disadvantages of existing algorithms and provide the motivation for this paper.
Adaptive Gaussian Sum Filter
In the traditional Gaussian sum filter , the a posteriori pdf is approximated using a weighted sum of Gaussian pdfs. The weights associated with the individual Gaussian components are selected based on the measurement likelihood function and are kept constant during state propagation. In this section, we summarize a recently developed method [34, 351 to update the weights of different components of the Gaussian mixture of equation ( 14) during propagation .
3. George, Terejanu, and Singla
The system model given in equation (12) can be written as a stochastic differential equation of the fonn
dx(t) ~ f(x(t) , w(t)dt + g(x(t))dB(t) (30)
where d:B(t) is an increment of Brownian motion process with zero-mean, Gaussian distribution and £[d'B(/)dllT(r)] = Qdt. Given the stochastic differential equation in equation (30), the conditional pdf, p(x,I Yk). satisfies the Fokker-PlanckKolmogorov equation between observations, i.e.
u - p(x,1 y,) ~ L",,{P(x,1Y,)} at
where LTp{ . } is the so called Fokker-Planck operator, and
L"" ~ - 2: ' , + 2: 2: ", [ 6 iJo(ll(t x) 6 6 a2Dm(t X)]
i - I ax; i - I j _ 1 ax/hi
DOl(t, x) ~ f(x(t), wet))
1 D"'(t, x) ~ "2 g(x(t))Qg' (x(t))
(31)
(32)
(33)
(34)
We mention that equat ions (31) - (34) represent the ItiJ illlerprerurioll for the FPK equation and are more popular among engineers and physicists. An altemalive interpretation (Stratollovich interpretation) for the FPK is also commonly used among mathematicians and leads to the same equation if g( .) is state independent. In this paper, we use the ItO interpretation for the FPK.
The main idea behjnd the adaptive Gaussian sum filter is that the mixture pdf jJ(t , xIY.) of equation ( 14) should satisfy the FPK equation between observations. Thus the weights associated with the individual Gaussian pdf are updated during propagation so that the FPK residual , e(x,). is minimized. The FPK residual is defined as
ap (x,1 Y,) , , (x,) ~ - L",, (p(x,1 y,)) at (35)
where
ap(x,IYk) = ~ i [ opg; T • I + ~ ~ 0Pg; p I.ik] L"W,!* i p. L.. L" ;Jk
at ,- 1 ap. t\*. i - I k- I apt\*. (36)
where pi,jk is the jkt" element of the it" covariance matrix p i, Further, substitution of equations (33) and (34) along with equation (36) in equat ion (35) leads to
N
,(x,) ~ 2:w~L; (x,) ~ L'w~ (37) i - I
where W,lk is a N x I vector of Gaussian weights. and L i is given by
L ;(x ,) ~ [ ap~;, jL; + ± ± ap,;, p,,,] UP. '1k j - I . - 1 ap'~
~(f ( _lap,; a~(x" w) 1 ~ + L" j X" W - + Pgi - -L"
j - l ax) aXi 2 k- I
'D"'() ) a jk x, P gI
Xj Xk (38)
Spacecraft Attitude Estimation Using Adaptive Gaussian Sum Filter 3.
Funher, different deri vatives in the above equation can be computed using the analytical formulas
op" _ ( ' )_'( ' ) -;;-;- - P,l~ x, - "" Ik Pgi u".~
:~; = ';i (p ,I,t)- I[(X, _ ",~)(x, - ",~)T(p~rl - h X6]
iJp" = _(P.\t)- I(X - "'~")P'i ox iJlPKI - = _(~~)- 1 [l6X6 - (x - ",~)(x - "':!l:f(p.\tr 1]p'i iJxxT
Now, at a given time instant , after propagating the mean , "'~ , and the covariance , PAt. of individual Gaussian elements using equations ( 16) and (17). we seek to update we ights by minimi zing the FPK residual over some volume of interest V [34 , 35 1
1 f '( ) ~( ' " min - e- x dx + £.. w~ .. - W~,k)-.. ~ 2 v I- I
N
S.t LW},l = 1
w~;?:: O, i = I , .... N (J9)
Here. the second term in the cost func tion is introduced to penalize large variations in the weights betwee n two time steps. The FPK residual given by equation (37) is li near in Gaussian weights. w~ . hence, the aforementioned problem can be written as a quadratic programming problem
min -.!.. wXt LwlIt + (wilt - Wl;A:)T(WI/J; - WtIt) .~ 2
s.t t1xI W,,l = I
wilt;?:: ONXI
where I Nx l is a vector of ones, ONXI is a vector of zeros and L is given by
L ~ f i(x)i '(x)dx , fL1Lldx f i ,i ,dx f LNL ldx , , , fi ,i,dx f i ,i ,dx f i .i,dx , , ,
fLIL,\dx fi,.c.,~x f LNL,\dx u , ,
(40)
(4 1)
In references [34. 35]. it shown that the aforementioned problem leads to a convex optimization problem and is guaranteed to have a unique sol ution . For the attitude estimation problem. the expression for L is
i , ~ N(x, - ". :,. P~)({f'(". :,., w ) - f' (x,. w)}(p:,)- '[x, - ".:'1 J [(W - b~) ] I ' + - xJ 0 - - LCfdx.TMlX, + I} 2 }x . 8 *_1
40 George, Terejanu, and Singla
+ ~ ± ±{(J"~) - '[x, - ",:.J[x, - ",:,r(P'~)- ' - (P~)- '}.,{A:, P'" 2 k• 1 j _ 1
+ p'~(A :.)T + g(",:')QgT(",:,) - g(X,)QgT(X')}.i)
where M 1, M 2 , and M] are diagonal matrices
MI = diag[15 -5 - 5 0 0 OJ Mz = diag[ - 5 15 - 5 0 0 OJ MJ = diag( - S - 5 15 0 0 OJ
Since f (.) is polynomial in nature, the various integrals in equation (41) can be related to higher order moments of various Gaussian components. The minimization problem will substitute for equation (19) of the conventional Gaussian sum filter whenever an estimate has to be computed, even between measurements. The summary of this adaptive Gaussian sum filter with forecast weight update for the continuous-time nonlinear dynamical systems and discrete measurement model is presented in Table I .
Numerical Examples
In this section, we demonstrate the effectiveness of the proposed Gaussian sum filter, developed in this paper, by simulating star-camera images . An 8" X 8" FOV star-camera is simulated by usi ng the pinhole camera model dictated by equations (I) and (2). The effective foca l length of the star-camera is assumed to be 65 mm for both FOYs.
TABLE 1. Summary of the Adapth'e Gaussian Sum Filter
Continuous-time nonlinear dynamics:
xCt) = r(X(t). W(I» + g(x(t))r(t)
Discrete-time measurement model: y ~ - h (x~) + VI
Propagation:
jJi (t) '""' r (p.j(t). w et»~ pitt) = Ai(t)P'(t) + P'(t)A jT(t) + g(,.i (t)) QgT(p.'(t))
Ai(t) = iJ::.:' C""C",')",' w",-c",t))1 ax(t) x(t) = p..;(t)
W~I = arg min .!. w~ Lwl,}; + (WI'; - W~:A-)T(WI~ - wl!~) •• 2
subject to iJx, w':A- = I
W,~ 2: ONX' Measurement -u lJdate:
p..l!l - p.l\i- , + KHy, - h(p.. llt - d) Ph = {hX6 - KV-r;}P~_ ,
Hi = ah(x~) I ax. '~ -"h-,
Kt. = Pi:A-_,(H:)T{HIPi',l_,(HilT + RI )"'
. wl)- ,/31 w~ = N ' .
Ij_ , Wl~ _ , /3'
f3 i: - N{YI - h(p..:,_,) .H{ Pi .. _,(HIY + Rt )
Spacecraft Anltude Estimation USing Adaptive Gaussian Sum Filter 41
For simulation purposes, the spacecraft is assumed to have an angul ar veloc ity in the body-fixed reference frame of
~ = 24 X 3600 rad/ sec (42)
The gyro data are simulated by assuming gyro data frequency to be 100 Hz and corrupted by gyro bias of
b ~ [0.30 0.50 0.20]' deg/he (43)
and Gaussian white noise according to equations (8) and (9) with values for the noise properties chosen as
q2 = q2 = q3 = J.6 X 10- 9 (rad/secl
q4 = q5 = q6 = 1.55 X 1O- 16(rad/sec2)2
To show the effectiveness of the proposed idea , we consider three test cases with star data update frequencies of 20 Hz and 10 Hz. It is assumed that five line-ofsight vectors are avai lable at all measurement update times for both test cases. Also, the true line-of-sight vectors are corrupted by Gaussian white noise of standard deviation 20 m-rad for both test cases .
In all the test cases we cons ider two Gaussian components in our mixture . and we run three filters : EKF, traditional GSF and the adaptive GSF. The covariances of the mixands in bolh test cases are set to P(to), which is assumed 10 be 7.615435 X 1O- 5h x3 rad 2 for the MRPs and 9.4017722 X 10- 4
iJX3 (rad/sec2)2 for
the bias terms. The initial weights of the two Gaussian components are be sel to W I = 10- 6 and W 2 = I - 10-6 for both cases. To compare the EKF with the Gaussian sum fi lters we run only the component with the largest we ight, here component number two. Thus the only difference between the test cases is the in itial condition for the state of the system and the frequency of the observations.
For the first case the initial condition for the state is selected as
x' ('o) ~ [1 deg 1 deg - 1 deg 30 deg/he 50 deg/he 20 deg/he]'
x2(to) = [5 deg 5 deg - 5 deg 15 deg/hr 25 deg/hr 10 deg/ hrr
The results for the first case are shown in Fig. 1, where the error in the atti tude and the error in Ihe bias for the conventional GSF and the proposed adaptive GSF are plotted. Both plots indicate that the adaptive Gaussian filter is significantly bener Ihan the conventional Gaussian sum fi lter. In Fig. 2, the corresponding weights of the Gaussian components are plotted for the traditional GSF and the adaptive GSF, respectively.
(a) AllilOOC Error
I • I I , ,
(h) Ilia, Error
FIG. I. RMS Error for TI'SI Case I (20 H7.).
-----
42 George, Terejanu, and Singla
(a) Con\cnlionai W~ighlS (II) Adapl;,X: Wcighh
FIG. 2. Weights of Gaussian Componems for Test Case I (20 Hz).
In (his particular case the EKF is not specifically considered . because the classical aSF behaves in this case as an EKF. The measurements are in agreement with the second component and have virtually no effect on the weight of the first cornjXlnent. Combined with roundoff errors, this becomes rather dangerous when the measurements boost the weight of one of the components to 1.0, making the weight of the other component 0 .0. Once this has happened the traditional GSF has no other mechanism to restore the weight of the Gaussian component with zero weight. The only equation which modifies the weights is equation (26). Unfortunately once the prior weight becomes zero it will remain zero from that time on, since in equation (26) the prior weight multiplies the likelihood of the measurement. Observe that in this case the uncenainty propagation plays an important role in updating the weights of the Gaussian components, reducing the error in the state estimate and bias.
For the second test case we choose the initial condit ion for the state as
xl(to) = [5 deg
x' (t. ) ~ [10 deg
5 deg -5 deg 30 deg/hr 50 deg/hr 20 deg/hr]T
10 deg - 10 deg 15 deg/hr 25 deg/hr 10 deg/hr]T
For this test case we also consider the EKF for comparison purposes. In Fig. 3 we observe that the traditional GSF has a slight improvement compared with the EKF, however the time update step plays an important role in minimizing the error in the state and bias as seen in Fig . 4 due to the adaptation of the weights in the adaptive GSF.
As in the first example, the adapti ve GSF recognizes the initial importance of the first component by updating its weight in the propagation step and providing an in crease in accuracy for the state estimate and bias. Hence the weight update scheme presented here becomes rather important when the frequency of the measurement is low, the noise in the measurement model is large and we are forced to rely more on the propagation step .
The last test case is sim ilar to the second one except that the measurement fre quency is 10 Hz here. Again as in the first case the EKF has not been considered since the classical GSF behaves in this case as an EKF, as shown in Fig. 6(a). Here even though the frequency of the measurements is lower than in the second case, the adaptive GSF is able to correct the weights in the propagation step , Fig. 6(b).
Both the error in the att itude and the error in the bias are smaller for the adaptive GSF than for the conventional one, shown in Fig. 5. Thus the adaptive GSF gives a better accuracy in-between the measurements due to its weight update scheme, whi le the classic GSF enters into a degenerate behavior similar with the one in the first case.
Spacecraft Attitude Estimation Using Adaptive Gaussian Sum Filter 43
I· I' (\
'.
-------._--~
v~=-. '-,-
--~ ! i I ,
-- ~ ..
(al AnilOOc Error (bl Hia; Error
FIG. 3. RMS Error for Test Case 2 (2{l H:t).
--------,.
'-"'"
-_. ~, ---. .. -~,
I
Il-f
.-.. _-.... _.
(a) C",,'cnliOl,.1 Wcil'hlS (h) Adapliw Weil'hlS
FIG. 4. Weights of Gaussian Componems for Test Case 2 (20 Hz),
_'-'" -C-O"" ---_... ---' ----,--
(a) A11ilude Error
, I I ,
FIG S. RMS Error for Test Case 3 (10 Hz).
'--
---. "C---' .: .~
'~""
-.-.. -...... ..
(h) Hia' Error
-.-
_. --' (a) Conwmional Weights (b) Adapl;'c Wcil'ht,
FIG.6. Weights of Gaussian ComponentS for Test Case 3 (10 Hz).
44 George, TereJanu, and Single
Concluding Remarks
In th is paper an adapti ve Gaussian sum fil ter algorithm is developed for the attitude estimation problem. The mean and covariance of each Gaussian component are propagated by using the extended Kalman filter while the weights associated with each component are updated by constraining the Gaussian sum approximation to satisfy the Fokker-Planck-Kolmogorov equation . During the measurement update, Bayes ru lc is used to update the weights of different Gaussian components. The perfonnance of the proposed method is compared with the pcrfonnance of the conventional Gaussian Sum Filter method and it is shown that the adaptation of the weights during pure propagation between two measurement time steps improves the performance of the conventional Gaussian sum filter. A major advantage of the adaptive Gaussian sum filter is that it not only provides the accurate state estimates but it also provides accurate estimates for both transition and a posterior pdfs which can be used for control design or for decision maki ng purposes .
References [II WERTZ. J. R. Spaceaaft Allitude Detennination (lIId COn/rol, Dordrccht: Kluwer Academic
Publishers. 1978. [21 SIDI, J . M. Spacecraft Dynamics and Comrol. Cambridge. UK: Cambridge Univers ity Press.
1997. [31 GERALD, L.M . "Three-Axis Attitude Determination." The Journlll of the AstronautiCilI
Sciences. Vol. 45. April-June 1997, pp. 195- 204. [4] SHUSTER. M.D. and OH . S.D. "Three Axis Attitude Determination From Ve<:tor Observa
tions:' Journal of Guil/ance (md Comrol, Vol. 4, No. I. 198 1. pp. 70- 77. [5] MORTAR!. D. --ESOQ: A Closed-Form Solution to the Wahba Problem: ' The Journal of the
AstronawiCilI Sciences. Vol. 45. April -June 1997. pp. 817- 826. [6] MORTARt. D. "Second Estimator of The Optimal Quatemion:' Journal of Guidance. Comrol
and Dynamics. Vol. 23. September- October 2000. pp. 885- 888. [71 LEFFERTS. E.J .. MARKLEY. F.L.. and SHUSTER. M. D. "Kalman Filtering For Spacecraft
Attitude Estimation:' Jounwl of Guidmlce. Comrol (I/ld Dynamics. Vol. 5. Sept.- Oct. 1982 . pp.417-492.
[81 BAR-ITZHACK . I. Y. " REQUEST: A Re<:ursive QUEST Algori thm for Sequential Allitude Detennination." Joumal oj Guidance. Comrol and Dynamics. Vol. 19. NO.5. 1996.pp. 1034-1038.
[91 CRASSIDIS. 1.L. and MARKLEY. F. L. "Unscented Filtering for Spacecraft Altitude Estimation." Journal ofGrlidance. Control and Dpwmics.Vol. 26. No.4. 2003. pp. 536-542.
[101 CHENG. Y. and CRASSIDIS. 1. L. "Particle Filtering for Sequential Spacecraft Attitude Esti mation:' prescnted as paper AIAA-2004-5337 at the AtAA Guidance . Navigation. and Control Conference. Providence. R!' 2004.
I III CRASSIDIS. J. L. , MARKLEY . F.L.. and CHENG. Y. "A Survey of Nonlinear Attitude Est i-mation Methods:' Joarnal ofGuiilmlce. Cottlroi (lim Dynamics. Vol. JO.No. 1.2007 . pp. 12-28.
[121 JAZWINS KJ , A. H. Stochastic Processes alld Filterillg Theory. Academic Press. 1970. [131 ANDERSON, B. D. and MOORE. J . B. Optimal Fillering. Prentice-Hall. 1979. [141 GELB. A. Applied Optimal ESlimation. MIT Press. 1974. [151 DOUCET. A .. FREITAS, N.d., and GORDON. N. Sequential Mottle-Carlo Methods in Pmc
tice. Springer-Verlag. April 2001. [16) IYENGAR . R. N. and DASH, P. K. "Study of the Random Vibration of Nonl inear Systems by
the Gaussian Closure Technique:' Jormllli of Applied Meclrallics. Vol. 45. 1978. pp. 393-399 . [171 ROBERTS , 1. B. and SPANOS. P. D. Random Vibration and Statistical Linearization. Wiley.
1990. [18) LEFEBVRE, T. , BRUYNINCKX . H., and SCHUTfER. 1.D. "Kalman Filters of Non-Linear
Systems: A Comparison of Performance:' hrtenralional }ounral of CO/lfrol. Vol. 77. No.7. 2004. pp. 639 - 653.
119J LEFEBVRE. T., BRUYN INCKX. H .. and SCHUTIER.1. D. "Comment on A New Method for the Nonl inear Transformations of Means and Covariances in Filters and Estimators." IEEE Transactions OIl Automatic CO/lfrol. Vol. 47. No.8. 2002.
Spacecraft Attitude Estimation Using Adaptive Gaussian Sum Filter 45
(201 RiSTIC. B .. ARULAMPALAM. S .. and GORDON. N. Beyond Ihe Kalman Filler: Particle Filters for Tracking ApplicUlions. Artech House. 2004.
(21 J DAUM. F. and HUANG, J . "Curse of Dimensionality and Particle Filters:' Pro<'eedit!gs of the ZOO3 IEEE Aerospace Confere/l ce, Vol. 4. March 8- 15. 2003, pp. 1979-1993.
(221 RISKEN. H. The Fokker·P/a/lck Equatiot!; Methods of Solution and Applications. Springer. 1989.
[23] DAUM. F.E. "Exact Finite Dimensional Nonlinear Filters:' IEE£ Transuctions on A"lOmatic Control, July 1986.
(24] FULLER. A. T. "Analysis of Nonlinear Stochastic Systems by Means of the Fokker-Planek Equation:' il1lemarional l oumal of COII/roi, Vol. 9. 1969. p. 6.
[25] KUMAR. M., SINGLA, P .. CHAKRAVORTY, S., and JUNKI NS . I . L. "A Multi-Resolution Approach for Steady State Uncertainly Determination in Nonlinear Dynamical Systems:' Proceedings of tile 38th SoU/heaslem Symposium on Syslem Theory, 2006.
[26] KUMAR. M .. SING LA. P.,CHAKRAVORTY, S .. and JUNKINS, J . L. "The Partition of Unity Finite Element Approach to the Stationary Fokker-Planck Equation ." Proceedings of II,e 2006 AIANAASAslro</)'IlUmics Speciaiisl Conference and Exhibit. Keystone . CO. Aug. 21 -24. 2006.
(271 MUSCOLINO. G .. RICCIARDI. G .. and VASTA, M. "Stationary and Non-Stationary Proba bility Density Function for Non-Unear Oscillators." Interrwtiono/ loum,,/ of NOli -Linear Mechanics. Vol. 32, 1997.pp. 1051-1064.
(28J PAOLA. M. D. and SOFi. A. "Approximate Solution of the Fokker-Planck-Kolmogorov Equation." Probabilistic Engineering Mec/ulflics. Vol. 17 ,2002, pp . 369- 384.
(291 MARKLEY , F. L. "Attitude Filteri ng on SO(3)," presented as paper AAS-05-460 at the AAS Malcolm D. Shuster Astronautics Symposium. Grand Island, NY. June 2005.
[30J SORENSON, H. W. and ALSPACH, D. L. " Recursive Bayesian Estimation using Gaussian Sums," AutonUllica, Vol . 7. 197\, pp. 465 - 479.
(3 11 ALSPACH. H. and SORENSON. D. "Nonlinear Bayes ian Estimation using Gaussian Sum Approximations:' I£EE TrallJactions 011 Automa/ic COII/roi. Vol. \7, No.4. \972, pp. 439-448 .
(321 ITO. K. and XIONG, K. "Gaussian Filters for Nonlinear Filtering Problems," IEEE TrallsactiO/IS 011 Automatic Comrol. Vol. 45, No.5, 2000. pp. 9 \0- 927.
[33] TEREJANU, G., SINGLA, P., SINGH. T.. and SCOlT. P. O. "A Novel Gaussian Sum Filter Method for Accurate Solution to Nonlinear Filtering Problem," The J Ith Imema/ional COllference on hl/onna/ion FUJion. Cologne, Germany, June 2008.
[34] SING LA. P. and SINGH , T. "A Gaussian function Network for Uncertaint)' Propagation through Nonlinear Dynamical System," Proceedings of Ihe IStII AA5I'AJAA Spaceflighl Mec/lUnics Meeting, Galveston, TX.Jan. 27 - 31. 2008.
[35] TEREJANU, G., SINGLA. P., SINGH. T.. and SCOlT. P.O. "Uncertainty Propagation for NOnlinear Dynamical Systems u~ing Gaussian Mixture Models." jO/lnUlI ofGuid(lIIce. Control. and Dynamics. Vol. 31,2008 . pp. 1623 - 1633 .
[361 SINGLA. P .. GRIFfITH. T. D.,and JUNKINS, I. L. "Atti tude Determination and On-Orbi t Autonomou.~ Calibration of Star Tracker For GIFfS Mission," Proceedings of the AASiAlAA Space Flight Mechanics Meeting (K. T. Alfriend. B. Neta. K. Luu. and C. A. H. Walker, cds.). Adl"Ullcts ill Aerospace Sciences, Vol. 112.2002. pp. 19- 38.
)371 SHUSTER. M. D. "A Survey of Attitude Representations:' The loun/a/ of the Astrollautical SciellceJ. Vol. 41. October- December 1993. pp. 439-5 17.
(38] JUNK INS.J. L. and SINGLA, P. " How Nonlinear Is It? A Tutorial on Nonlineari ty of Orbi t and Attitude Dynamics: ' The loumal of Astrollaulica/ Sciellces. Vol. 52, No. 1-2, 2004. pp. 7 - fl).