Estimating Attitude from Vector Observations Using a Genetic Algorithm-Embedded Quaternion Particle Filter

Transcription

1 AIAA Guidance, Navigation, and Control Conference and Exhibit 6-9 August 24, Providence, Rhode Island AIAA Estimating Attitude from Vector Observations Using a Genetic Algorithm-Embedded Quaternion Particle Filter Yaaov Oshman and Avishy Carmi Technion Israel Institute of Technology, Haifa 32, Israel A novel algorithm is presented for the estimation of spacecraft attitude, represented by the rotation quaternion, from vector observations. Belonging to the class of Monte Carlo sequential methods, the new estimator is a particle filter that uses approximate numerical representation techniques for performing the otherwise exact time propagation and measurement update of potentially non-gaussian probability density functions pdf) in inherently nonlinear systems. The paper develops the filter and its implementation in the case of a low Earth orbit LEO) spacecraft, acquiring noisy Geomagnetic field measurements via a three-axis magnetometer TAM). A genetic algorithm is used to estimate the gyro bias parameters, avoiding the need to expand the particle filter s state. This renders the new estimator highly efficient and enables its implementation with a remarably small number of particles. Contrary to conventional filters, that have to address the quaternion s unit norm constraint via special mostly ad hoc) techniques, the new filter maintains the quaternion s unit norm naturally, requiring no such modification. The results of an extensive simulation study are presented, in which the new filter is compared to two conventional extended Kalman filters and to the recently proposed unscented Kalman filter. The comparison demonstrates the viability of the new algorithm and its superior convergence rate relative to the competing estimators. An example using real data from the Technion s Gurwin TechSAT satellite is presented as well. In this example, the TAM s measurement noise is non-gaussian. It is shown that, while presenting significant difficulties to the extended Kalman filter implementations and to the unscented Kalman filter, this peculiar measurement noise has no adverse effect on the particle filter. The higher efficiency in the usual statistical sense) of the particle filter relative to that of the extended and the unscented Kalman filter implementations is shown by comparing their covariance matrices to the Cramer-Rao lower bound. I. Introduction USING a sequence of vector measurements for attitude determination has been intensively investigated over the last four decades. First proposed in 965 by Wahba, the problem is to estimate the attitude of a spacecraft based on a sequence of noisy vector observations, resolved in the body-fixed coordinate system and in a reference system. Body-fixed vector observations are typically obtained from onboard sensors, such as star tracers, Sun sensors, or magnetometers. Corresponding, reference observations, are obtained by using an ephemeris routine for a Sun observation), or from orbit data and a magnetic field routine for a magnetic field observation), or from a star catalog for star observations). Inertial reference systems typically utilize vector measurements in combination with strap-down gyros to estimate both the spacecraft attitude and the gyro drift rate biases. Several approaches have been proposed for the design of such systems, differing mainly in their choice of attitude representation method. The quaternion, a popular rotation specifier, was used in the framewor of extended Kalman filtering EKF) in Ref. 2 using the so-called multiplicative approach) and in Ref. 3 using the additive approach). The incorporation of the QUEST measurement model within a Kalman filter s measurement update stage was Associate Professor, Department of Aerospace Engineering. Associate Fellow AIAA. yaaov.oshman@technion.ac.il. Doctoral Student, Department of Aerospace Engineering. avishy@aerodyne.technion.ac.il. of 24 Copyright 24 by Y. Oshman and A. Carmi. Published by American the American Institute Institute of ofaeronautics and and Astronautics, Astronautics Inc., with permission.

2 presented in Ref. 4. In Ref. 5, vector observations were used to estimate both the quaternion and the angular velocity of the spacecraft, in a gyroless attitude determination and control setting. The main advantage of using the quaternion representation is that it is not singular for any rotation. Moreover, its inematic equation is linear and the computation of the associated attitude matrix involves only algebraic expressions. However, the quaternion representation is not minimal since it is four-dimensional. This leads to a normalization constraint that has to be addressed in filtering algorithms. Thus, Ref. 2 assumed that the 4 4 quaternion estimation error covariance matrix must be singular an assumption which is not true in the strict sense in the case under consideration) and proposed reduced-order algorithms in order to maintain the assumed singularity. On the other hand, Ref. 3 assumed no such singularity, but incorporated a normalization stage within its EKF algorithm, which renders the resulting estimator strictly non-optimal and increases its worload. In a recent paper, 6 an unscented Kalman filter UKF) was proposed for the estimation of the rotation quaternion. The UKF does possess a reported advantage over the EKF with regards to dealing with strongly nonlinear systems, since it avoids the linearization associated with the EKF. However, since using the UKF directly with the quaternion attitude parameterization would also yield a non unit norm quaternion estimate after all, the UKF is still a Kalman filter), the authors of Ref. 6 chose to wor with a generalized three-dimensional attitude representation, still using the quaternion for updates in order to maintain the normalization constraint. It should also be noted that, as a Kalman filter mechanization, the UKF is also sensitive to the statistical distribution of the stochastic processes driving the dynamic model: non-gaussian distributions guarantee non-optimality of the estimates. Extending the results of the wor previously presented in Ref. 7, this wor proposes to sequentially and directly estimate the rotation quaternion from vector observations using a particle filter PF). Also nown as sequential Monte Carlo SMC) methods, particle filters refer to a set of algorithms implementing a recursive Bayesian model using simulation based methods. 8 Avoiding the underlying assumptions of the Kalman filter, namely, that the state space is linear and Gaussian, these rather general and flexible methods enable solving for the posterior probability distributions of the unnown variables on which all inference on these variables is based) within a Bayesian framewor, exploiting the dramatic recent increase in computer power. It should be emphasized that PFs are not just smart implementations of the Kalman filter or its nonlinear variants/extensions; rather, they are entirely different algorithms that lead to entirely different solutions to the nonlinear, non-gaussian filtering problem. Contrary to the Kalman filter extensions, the solutions obtained using PF algorithms are approximations to the optimal in the Bayesian sense) solutions, which can be made arbitrarily close to the exact solutions by increasing the number of particles involved in the computation, thereby also increasing the computation worload. Estimating the attitude quaternion using a PF enjoys three major advantages relative to existing estimation methods. First, the estimator wors directly with the quaternion i.e., its particles are all attitude quaternions). This inherently renders its resulting estimate an attitude quaternion, naturally satisfying the norm constraint requiring no special procedures for this purpose). Second, since the PF algorithm assumes nothing on the noise distributions, the resulting algorithm can wor with any noise distribution associated with the particular sensors involved. In contradistinction, the UKF assumes Gaussian distributions of the driving noise processes, which should not always be true. In fact, the PF algorithm does not propagate and update the covariance, completely avoiding the question of singularity although the covariance can be computed at any moment if needed for output monitoring purposes). Finally, the PF is easy to implement, and is insensitive to the initial conditions and to the nonlinearities involved. On the other hand, implementing the PF with a large number of particles to achieve high accuracy) can lead to an impractical computational burden. The remainder of this paper is organized as follows. The next section presents a brief introduction to SMC methods. Next, the mathematical model of the quaternion estimation problem is outlined. Section IV provides a detailed development of the quaternion particle filter for the case when the gyro biases are not estimated. The extension of the algorithm to compute the maximum lielihood estimate of the gyro biases via the use of a genetic algorithm then follows in the next section. A derivation of the Cramer-Rao lower bound for this problem is outlined in section VI. Section VII presents the results of an extensive simulation study that was carried out to assess the performance of the new algorithm and compare it to existing quaternion estimators. Finally, concluding remars are offered in the last section. 2 of 24

3 II. Sequential Monte Carlo Filtering The optimal solution of the nonlinear estimation problem involves an accurate propagation of the optimal probability density function PDF), namely, the conditional PDF of the state given the observation history. Due to the complex nature of nonlinear estimation problems, many estimation algorithms rely on various assumptions to ensure mathematical tractability. It is well nown that the famous Kalman filter is the optimal estimator for linear Gaussian state-space models, but its performance is limited when the aforementioned assumptions do not hold. The optimal PDF admits a Bayesian recursion, which means that it is propagated in accordance with some prior distribution of the state and a lielihood function that relates the states to the incoming observations. In the case of linear Gaussian models, where the PDF can be characterized by its first two moments, the Bayesian approach yields the Kalman filter. In general, for nonlinear, non-gaussian models, there is no explicit, closed-form solution. Several approximate methods have been proposed. These include the EKF, the Gaussian sum filter and numerical integration over a state space grid. PFs or SMC methods) refer to a set of algorithms implementing a recursive Bayesian model by simulation-based methods. This involves representing the required posterior probability density by a set of random samples with associated weights, and deriving the estimates based on these samples. Unlie the other methods, SMC methods are very flexible, easy to implement and applicable in very general settings. A. Bayesian Inference It is assumed that the unobserved signal i.e., the process) {x, N}, where x X, is a Marov chain with a given initial distribution p x, which evolves according to a transition ernel p x x. The observations {y, N}, where y Y, are conditionally independent given the process, possessing the marginal distribution p y x. Denote by X = {x,..., x } and Y = {y,..., y } the process and observations histories up to time, respectively, and let X = {X,..., X } and Y = {Y,..., Y } be the realizations of X and Y, respectively. In most nonlinear filtering problems the goal is to estimate the marginal distribution p x Y filtering distribution) recursively in time. By applying the Chapman-Kolmogorov equation and Bayes formula, the following recursion is obtained for the filtering PDF p x Y X Y ) = p x Y X Y ) = + + p x x X X )p x Y X Y )dx a) p y x Y X ) p y x Y X )p x Y p X Y x Y )dx X Y ) b) In most cases one cannot obtain the normalizing distribution p Y Y and the marginals of the posterior p X Y. Thus, these expressions can rarely be used in a straightforward implementation. Instead, approximations should be utilized, using alternative methods. B. Particle Approximation Assume that it is possible to sample N independent and equally weighted random samples called particles ) {X i)} N i= of the posterior p x Y. Hence the continuous filtering density can be approximated by a discrete one. From the strong law of large numbers LLN), as N, this approximation is equivalent to the posterior with probability w.p. ). An empirical estimate of the expectation of some bounded function fx ) is, therefore, the sample mean N N i= fx i)): lim N N N i= fx i)) = E [ fx ) Y ] w.p. 2) Unfortunately, one usually cannot sample efficiently from the posterior p x Y. This problem can be alleviated by adopting a sampling technique called importance sampling. This technique consists of introducing an arbitrary so-called importance distribution π x Y a. Then, if one can sample efficiently from π x Y, an a The selection of such function can consist of minimizing the variance of the estimate in Eq. 2). It can be shown that var ) N N i= w i)fx i)) is minimized by choosing π ) such that w i) is nearly constant for all i =,..., N 3 of 24

4 approximation to the expectation in Eq. 2) is obtained by N N i= w i)fx i)) where w i) p x Y is the importance weight of the ith particle. Let π x Y ) X i) Y ) X i) Y 3) w i) = w i) N j= w j) 4) be the normalized importance weight of the ith particle, then it can easily be verified that lim N N i= w i)fx i)) = E [ fx ) Y ] w.p. 5) The implementation of particle approximations using a finite number of samples raises some practical difficulties. 8 The so-called particle degeneracy problem refers to a phenomenon encountered when using the importance sampling method, when after a few iterations all but usually one particle have zero weights. Over time, the distribution of the importance weights becomes more and more sewed, which consequently means that much computational effort is devoted to updating state trajectories whose contributions to the final estimate are close to zero. To avoid the problem of degeneracy a so-called selection/resampling step is introduced into the algorithm. C. Selection/Resampling Selection/resampling consists of discarding state trajectories whose contributions to the final estimate are small, and multiplying trajectories whose contributions are expected to be significant. This means regeneration of particles with large importance weights and eliminating those with small importance weights. The resampling procedure decreases algorithmically the particles degeneracy, but introduces some practical problems. During the resampling procedure the more liely particles are being multiplied, so that the particle cloud is concentrated in regions of interest of the state space. This produces a new particle system in which several particles have the same location. Moreover, if the dynamical noise is small, the particle system ultimately concentrates in a single point in state space. This loss of diversity will eventually prevent the filter from correctly representing the posterior. One way of maintaining the particles diversity is by injecting artificial process noise into the system. This technique is nown as regularization, or roughening. D. Regularization Suppose that x R n, ν R n are two mutually independent random vectors with densities x Y p x Y and ν p ν. Regularization is carried out by defining the random variable x x + ν. Applying a simple manipulation, the density of the random variable x is evaluated using the densities of the two random variables x and ν as follows p x Y X Y ) = = + + p x x,y X X, Y )p x Y X Y )dx p ν X X )p x Y X Y )dx = E [ p ν x X ) Y ] 6) Thus, an empirical approximation of this density is p x Y X i) Y ) = N w j)p ν X i) X j)) 7) j= It is often convenient to simulate ν using a zero mean Gaussian ernel with a predetermined bandwidth parameter h, namely p ν N n, hq ), where Q R n n denotes the regularization intensity matrix. 4 of 24

5 When p x Y is Gaussian with a unit covariance, it has been shown 8 that the optimal bandwidth that minimizes the integrated mean square error between p x Y and p x Y is h o = [4/ Nn + 2))] n+4 8) If the underlying density covariance is not unity, then Eq. 8) can still be used by setting the value of the intensity matrix Q to covx Y ). E. Sequential MC Filtering Consider the following nonlinear discrete-time process and observation equations x = f x, u, v ) 9a) y = g x, n ) where x R n, u R m, y R p denote the state, the deterministic control and the observation processes, respectively. v R l and n R p are the mutually independent process and measurement white noises with nown densities p v and p n, respectively. The initial state is independent of both noises and has a nown probability density function p x. Using nown relations from probability theory, the transition density p x x and the lielihood p y x can be expressed in terms of the process and observation equations, respectively p x x X X ) = p v f X, X, u ) ) det v f x, u, v )) a) p y x Y X ) = p n g Y, X ) ) det n g x, n )) b) where v f, n g are the Jacobian matrices of f, g with respect to v and n, respectively, and the functions f, g are the inverse functions of f, g respectively. Let {X i)} N i= be a particle approximation of the posterior at time and let {w i)} N i= be the weights associated with this approximation. Then, applying Eqs. ) results in a new set of particles representing the posterior at time. First, the particle approximation of the prior p x Y is obtained via Eq. a) 9b) p x Y X i) Y ) = N j= p x x X i) X j)) w j)π x Y X j) Y ) ) In practice the simulation of Eq. )and Eq. a)) is performed by passing the particles of time through the process equation along with the injection of artificialy simulated process noise. It should be noted, though, that additional process noise is injected naturally when applying regularization, thus, in this case it is usually not necessary to inject additional noise. Now, given a new observation Y, the updated posterior approximation is obtained p x Y X i) Y ) py x Y X i)) w i)π x Y X i) Y ) 2) Choosing the importance distribution to be the prior π x Y from Eq. 2), the importance weights satisfy = p x Y, and letting π x Y = π x Y w i) p y x Y X i)) w i) 3) III. Mathematical Model A. Quaternion Process Model The truth-model discrete-time quaternion process equation is q + = Φ o q 4) 5 of 24

6 where q denotes the true quaternion of rotation from a given reference frame R onto the body frame B at time. The quaternion is defined on the 4-dimensional unit hypersphere S 3 and constructed from vector and scalar parts respectively q = [ϱ T q 4 ] T 5) The orthogonal transition matrix Φ o is expressed using ωo = [ωo ω2 o ω3 o ]T, the angular velocity vector of B with respect to R resolved in B. Assuming that ω o is constant during the sampling time interval t yields [ ] Φ o = Φω) o = exp 2 [ωo ] t 2 ωo t 2 ωot t 6) where the cross-product matrix associated with the vector ω o is defined as ω [ω ] o 3 o ω2 o ω3 o ω o 7) ω2 o ω o In practice, the true angular velocity vector ω o is not nown, but, rather, it is measured or estimated. In this wor it is assumed that the angular velocity vector is measured by a triad of gyroscopes. For this sensor, a widely used model is given by Ref. 2 as ω = ω o + η + ɛ 8) where ω denotes the measured angular velocity vector and ɛ, η are the gyro s measurement noise and bias vectors, respectively. The measured transition matrix, Φ, can be computed using ω instead of ω o in Eq. 6), and the quaternion process equation becomes where the process noise is incorporated through the transition matrix. B. Observation Model q + = Φ ω )q 9) Let r and b be a pair of corresponding vector measurements acquired at time in the two Cartesian coordinates systems R and B, respectively. Let A be the rotation matrix also nown as the attitude matrix or the direction cosine matrix) that brings the axes of R onto the axes of B at time. In general, the reference vector r is nown exactly, whereas the body vector b is measured. This results in the following attitude measurement model b = A r + δb 2) where δb is the measurement noise process, with nown density p δb. The relation between the rotation quaternion and the attitude matrix is given by A = Aq) = [q 4 ) 2 ϱ T ϱ]i ϱϱ T 2q 4 [ϱ ] 2) IV. Quaternion Particle Filter This section presents the quaternion particle filter QPF). The algorithm estimates the quaternion from pairs of vector observations. Within this particle filter, each particle is a unit norm quaternion, so that the norm constraint is inherently preserved. In the first stage, presented in this section, the QPF is derived assuming unbiased gyro measurements. Then, in the next section, the algorithm is expanded to incorporate gyro biases. A. Measurement Update Denote by Y = {[b ; r ],..., [b ; r ]} a set of measurements constructed from pairs of vector observations up to time. Given a realization q of the quaternion q at time, the measurement Y = [b ; r ] is 6 of 24

7 statistically independent of past observations. The lielihood of the measurement Y associated with a given quaternion is p y q Y q ) = p δb b Aq )r ) 22) Since, given q, the probability density of q is independent of past observations, the prior distribution at time conditioned upon past observations is obtained by p q Y q Y ) = + p q q q q )p q Y q Y )dq 23) When the measurement Y becomes available, the updated posterior at time satisfies p q Y q Y ) p y q Y q )p q Y q Y ) 24) Now let {q i)} N i=, {w i)} N i= denote N independent unit quaternion samples from the posterior at time, and their associated weights, respectively. Setting the importance distribution to be the prior PDF yields the importance weights as w i) = p y q Y q i)) w i) 25) Eq. 25) is refered to as the update stage. Still, in accordance with Eq. 23), one has to incorporate an evolution stage, as the samples need to simulate the prior PDF. B. Particles Evolution Passing the unit quaternion samples at time through the process equation results in a new set of samples. This is almost equivalent to applying Eq. 23) to the samples. The slight difference is due to the process noise distribution, which forms the transition ernel p q q. In the case of a relatively low-intensity process noise, the new quaternion samples thus obtained simulate the prior p q Y quite adequately. In other cases, the injection of an additional, artificial noise improves the set. C. Resampling In order to avoid particle degeneracy, a resampling procedure is implemented. The measure of degeneracy adopted here is the effective sample size. Introduced by Liu, 9 this criterion is defined using the variances of the importance weights. The effective sample size N eff is defined as An empirical estimate of N eff is ˆN eff = N N eff N E π [w 2 ] < N 26) N N i= w i) = 2 N i= w 27) i) 2 The resampling procedure is used whenever ˆN eff becomes less than a pre-determined threshold N th. The new set of samples is generated by resampling each particle q i) with probability w i). This consists of multiplying each sample according to its associated normalized weight. The number of offspring for each sample is evaluated by N i) = N w i), i =... N 28) To compensate for the loss of particles diversity, an artificial perturbation scheme based upon regularization is introduced into the algorithm. In this wor, the bandwidth of Eq. 8) is used. The adopted measure of regularization intensity is introduced next. 7 of 24

8 D. Regularization Intensity The rotation quaternion is defined only on the 4-dimensional unit hypersphere. Therefore, evaluating the covariance covq Y ) from the quaternion samples is meaningless. Instead, a measure for regularization intensity can be obtained by evaluating the 3 3 covariance matrix of the corresponding generalized Rodrigues parameters GRP) vector. Given a quaternion realization q, the associated GRP vector, denoted by ξ, is computed as ρ ξ = f 29) a + q 4 where ρ is the vector part of the quaternion realization i.e., the realization of ϱ), a [, ], and f is a scale factor. In this wor the values of a =, f = 2 + a) = 4 are chosen, so that ξ is equal to the Euler parameters for small angles. To evaluate the covariance of the GRP, let the filtered quaternion be denoted by ˆq, and the associated GRP by ˆξ. The covariance of ξ is then [ P ξ = E ξ ˆξ)ξ ˆξ) ] T Y 3) An empirical measure of this covariance can be obtained using weighted GRP samples, by ˆP ξ = N i= [ wi) ξi) ˆξ ] [ ˆξ] T ξi) 3) where ξi) denotes the GRP associated with the quaternion particle qi). An alternative approach for obtaining a measure of regularization intensity consists of evaluating the second moment of the quaternion multiplicative error, defined as δq q ˆq 32) where the denotes the quaternion product operator. The second moment of δq is [ q P q = E ˆq ) q ˆq ) ] T Y 33) An empirical measure for Eq. 33) can be obtained using weighted unit quaternion samples, by N ˆP q = wi) [ qi) ˆq ] [ qi) ˆq ] T i= 34) From Eqs. 33) and 34) one can see that the trace of P q and ˆP q is equal to. This allows us to define a so-called variance quaternion, constructed from the square roots of the diagonal elements of ˆP q ; that is, the components of this quaternion are ˆP q ) ii, i =,..., 4 35) leading to q vari q var 2 = tr ˆP q ) = 36) The meaning of q var becomes clear upon analyzing P q for small errors. In that case δq [θ T /2 ] T with θ [ϑ φ ψ] T, where ϑ, φ and ψ are three small Euler angles. Under this assumption, P q can be approximated as P q 4 E [ θθ T Y ] ) 3 37) 3 From Eq. 37) it is clear that, for small errors, q var = [ 2 var ϑ 2 var φ 2 var ψ ]T. 8 of 24

9 E. Regularization Scheme Denote by {q i)} N i= the stoc of N unit quaternion samples at time, where q i) = [ρ T i) q 4i)] T. Using Eq. 28), the number of offspring denoted by N i) is determined for each particle in the stoc. The quaternion offspring are then computed in the following manner: sample a set of vectors from a 3-dimensional zero-mean, unit covariance Gaussian or some other) ernel denoted by K, to get δξ o j) K, I 3 3 ), j =... N i)/2 38) The next step consists of rescaling and rotating the 3-dimensional space according to the regularization intensity measures obtained in the previous section. In the case where ˆP ξ is chosen to be the regularization intensity measure, each vector is rescaled and rotated according to δξj) = ± h ˆP ξ δξ o j) 39) where h denotes the regularization bandwidth, and ˆPξ is the matrix square root of ˆP ξ. Note that Eq. 39) defines a symmetric set of N i) vectors {δξj)} N i) j=. The associated GRPs of the quaternion offspring are then computed by 4 ξj) = + q 4 i) ρ i) + δξj), j =... N i) 4) Using the inverse transformation of Eq. 29) with a =, f = 4) yields the quaternion offspring denoted by qj) as [ ] T qj) = 4 + q 4j)ξ T j) q 4 j) 4a) where q 4 j) = 6 ξj) ξj) 2 4b) to If ˆP q is chosen to be the regularization intensity measure, each vector is rescaled and rotated according δθj) = ± h ˆP θ δξ o j) 42) where ˆPθ is the matrix square root of ˆP θ, and ˆP θ is the 3 3 matrix obtained by eliminating the last row and column of ˆP q. Note that Eq. 42) produces a symetric set of N i) vectors {δθj)} N i) j=. The quaternion offspring of the i th particle q i) are then evaluated by where qj) = δqj) q i), j =... N i) 43a) δqj) = [δθ T j) ] T 43b) After obtaining the quaternion offspring, each one should be weighted properly. This second stage weighting is crucial for the overall performance of the filter. The resampling procedure is, in fact, an external interference that injects random particles which have no past trajectories. A proper weighting of the offspring will reduce the effect of this contamination, while improper weighting of the offspring will worsen the posterior representation and, in some extreme cases, can cause divergence. Re-weighting can be carried out by using Eq. 7), so that the new particles are treated as if they were sampled from a continuous PDF. Another idea, which tends to give better results, is to re-weight the offspring proportionally to their lielihood. Thus, the second stage importance weights are being computed as w j) = c p y q Y qj)) 44) where the normalizing constant c is selected based on numerical considerations. A particle stoc with sewed importance weights can be improved in the next time step by properly choosing c. In this wor the value c = p y q Y ˆq ) was used, where ˆq denotes the filtered quaternion at time. 9 of 24

10 F. The Filtered Quaternion At time, N weighted unit quaternion samples are available. In order to obtain the filtered quaternion, the following minimization problem is solved min ˆ A N w i) A q i))  i= 2 F subject to  T  = I ) where  denotes the orthogonal attitude matrix associated with the filtered quaternion, and F is the Frobenius norm. Because C 2 F = tr{ct C} for any C R m m it follows that if  is orthogonal then the constrained minimization problem 45) is equivalent to the unconstrained minimization problem min ˆ A N i= which leads to the maximization problem max  N i= { w i) tr [ A q i)) T A q i)) ] tr [ T  A q i)) ]} 46) [ T w i) tr[ât A q i))] = max tr   N i= ] w i)a q i)) This maximization problem is nown as the orthogonal Procrustes problem. Letting and denoting by B 47) N w i)a q i)) 48) i= B = U Σ V T 49) the singular value decomposition SVD) of B, where U and V are the orthogonal singular vector matrices and Σ is the singular value matrix of B, the solution of the Procrustes problem is obtained as  = U V T 5) The filtered quaternion is then obtained from Â. An equivalent approach for minimizing the cost in Eq. 45) consists of writing Eq. 47) in quadratic form as follows with [ T max tr   N i= ] w i)a q i)) = max ˆq T Kˆq 5) ˆq [ ] B K + B T I 3 3 trb ) z B B 32 23, z B 3 B 3 52) z trb ) B 2 B 2 Now, similarly to the well-nown Davenport s q-method, the quaternion that solves the maximization problem of Eq. 5) is the normalized eigenvector corresponding to the largest eigenvalue of K. The quaternion particle filtering algorithm is finally summarized in Algorithm. V. Incorporating Gyro Biases Properly accounting for biased gyro measurements in the particle filter results in a higher dimensional model. A 7-state augmented model accounts for the quaternion dynamics and a 3-component bias process. A straightforward implementation of a particle filter for this model is computationally expensive because a prohibitively large number of samples might be needed to represent the posterior PDF. In this section a technique is presented to reduce the size of the state space by applying a genetic search algorithm. The resulting filtering algorithm is termed GA-QPF. of 24

11 Algorithm The QPF Algorithm for Unbiased Gyros : Perform particles evolution, to obtain a new set q i) = Φω)q i) for i =,..., N time propagation) 2: if new measurement Y is available then 3: Update the importance weights using Eq. 25) and normalize for all i =,..., N measurement update) 4: Compute the filtered quaternion using Eq. 5) 5: if N eff < N th then 6: Compute Regularization intensity using Eq. 3) or Eq. 34) 7: for all i =,..., N do 8: Compute the number of offspring N i) for particle q i) 9: Produce regularized weighted offspring as described in subsection E of the present Section. : end for : end if 2: else {Time propagation only} 3: Obtain the filtered quaternion using Eq. 5) 4: end if A. Approximation of the Lielihood The bias estimator developed in this wor is a maximum lielihood estimator, which is based on maximizing the lielihood L η Y ). The development relies on the assumption that the bias process evolves very slowly in time. Based on this underlying assumption it follows that η η for a finite time interval 2. Defining a batch of measurements corresponding to the specific time interval [, 2 ], that is Y 2 = {y,..., y 2 } with the realization Y 2 = {Y,..., Y 2 }, allows writing the lielihood function as L η Y 2 ) = 2 j= p yj Y j,η Y j Y j Assuming that the quaternion estimate represents well the information contained in the measurements on which it is based, the lielihood can be approximated as ), η 53) L η Y 2 ) 2 j= p yj q j ) 2 Yj Φ ω j η) ˆq j = j= p δb bj A Φ ω j η) ˆq j ) rj ) 54) where Φω j η) is the quaternion process transition matrix, evaluated for a given η. Eq. 54) requires the evaluation of the lielihood only once per time step for a given realization of η, thus enabling a real-time bias estimator to be developed. Finding the maximum lielihood ML) estimate of η that maximizes Eq. 54), requires the implementation of some maximization method. Avoiding the recomputation of the lielihood for every realization of η over the entire state sequence requires that the sequential maximization routine eeps trac in time of most liely candidates. B. Genetic Search Algorithms Sophisticated but simple, genetic algorithms GA) are search methods based upon the mechanization of natural selection and natural genetics. In much the same manner that the particle filter maintains the posterior, namely, by eliminating particles with zero weights and producing offspring by resampling, GAs combine survival of the fittest among string structures called chromosomes) with randomized information exchange. The common GA uses a coding of the parameter set, usually via a string, as opposed to woring with the parameters themselves. This enables the algorithm to exchange information between string elements in the population. Given an initial population of chromosomes and applying a single GA iteration results in an improved generation of chromosomes by means of lielihood. A simple GA consists of the following stages: ) reproduction, 2) crossover, 3) mutation. Reproduction is the process by which individual chromosomes are being reproduced according to their fitness function or lielihood function). Thus, more liely chromosomes will have higher probability of contributing offspring in the next generation. Crossover is the process of exchanging genetic information between two reproduced chromosomes. A simple crossover is carried out by of 24

12 selecting two chromosomes randomly and swapping all their characters from a randomly selected position to the total string length. Even though reproduction and crossover improve the population, they may become overzealous and lose potentially important genetic information. Mutation protects against such an irrecoverable loss by simply altering a character with small probability once in a while. C. Implementation of the GA Algorithm The QPF is modified to process gyro biases by introducing a simple GA into the algorithm. The proposed GA maximizes the lielihood in Eq. 54) sequentially in time and includes some modifications in order to cope with the varying nature of the biases. Coding The coding scheme used by the GA is the following. Denote a bias realization by Υ = [Υ Υ 2 Υ 3 ] T, where Υ j is the jth component of the vector Υ. The corresponding chromosome of length l + of the jth component, denoted by BΥ j ), is a string of length l + containing the binary coding of abscυ j ) and a sign bit. The chromosome of BΥ) is, therefore, a merged string of BΥ j ) for j =, 2, 3, that is BΥ) = { BΥ ), BΥ 2 ), BΥ 3 ) } 55a) where BΥ j ) = { sgnυ j ), bin{absυ j C) } 55b) where binυ j ) represents the binary coding of Υ j. For the sae of notational simplicity the sub-strings associated with each bias component are not individually designated in the proceeding sections. It should be noted, though, that the crossover operation swaps characters between the same string components and is applied to all the strings within the chromosome. Taing the initial set of parameters to be {Υi)} N η i=, the constant C is determined by C = max i Nη j=,2,3 2 l Υ j i)) 56) rendering the length of each chromosome corresponding to a component of the parameter set) l +. Sequential Lielihood Buildup The purpose of the GA is to produce an improved population by means of maximizing the lielihood. This requirement identifies the fitness function as the lielihood approximation in Eq. 54). Furthermore, recalling that every GA iteration produces a new population of parameters, clearly, for the lielihood of some Υ i) to be computed sequentially, it is essential to specify time intervals where the population does not change through generations in those time intervals there are no reproduction/crossover/mutation operations). Maintaining Υ i) = Υ i) for some 2 consequently facilitates the use of Eq. 54) over this time interval. Taing the fitness function to be the lielihood approximation implies the following recursion for the fitness function of any parameter {Υ i)} Nη i= : ϕ Υ i)) = p y q Y Φ ω Υ i)) ˆq ) ϕ Υ i)) = p δb b A Φ ω Υ i)) ˆq ) r ) ϕ Υ i)), 2 57) The fitness function is then normalized according to ϕ Υ i)) = ϕ Υ i)) Nη j= ϕ Υ j)), i =,..., N η 58) so that the number of offspring of each parameter is simply its fitness function multiplied by the total number of parameters which means that Υ i) is reproduced with probability ϕ Υ i))). 2 of 24

13 Reproduction At time the set of parameters and their fitness values are {Υ i)} N η i= and { ϕ Υ i))} N η i=, respectively. When a new measurement is available, applying Eqs. 57) and 58) updates the fitness value of each parameter. Each parameter Υ i) is then reproduced N i) times, where the corresponding number of offspring is obtained by The reproduced offspring for the parameter Υ i) are with the fitness N i) = ϕ Υ i)) N η, i =,..., N η 59) Ῡ j) = Υ i), j =,..., N i) 6) ϕ Ῡ j) ) = ϕ Υ i)) 6) Usually the reproduced population has slightly less than N η parameters due to the fact that N i) is not an integer. In that case one can reproduce the most liely parameter several more times. Perturbation This stage, which is optionally performed, enhances the robustness of the algorithm. Adopting the idea of offspring perturbation used in the resampling stage of the particle filter, an artificial Gaussian white noise is injected into the produced population. For any parameter Υ i), sample ζj) N 3, h η P η ), j =... N i)/2 62) where h η and P η denote some predetermined bandwidth and the sample covariance of the parameter set, respectively. The offspring of the ith parameter are then obtained for all j =,..., N i)/2 by Ῡ j) = Υ i) + ζj) Ῡ N i)/2 + j) = Υ i) ζj) 63a) 63b) Note: for an odd number N i), one of the offspring is Υ i).) The offspring eep the same fitness value of their parent, thus for all j =,..., N i)/2 ϕ Ῡ j)) = ϕ Υ i)) ϕ Ῡ N i)/2 + j))) = ϕ Υ i)) 64a) 64b) Crossover Decoding the reproduced set yields a population of corresponding N η chromosomes. Moving some chromosomes to a so called mating pool, the process proceeds by choosing two chromosomes randomly from this mating pool, and by choosing another uniformly distributed random number between 2 and l +, denoted by n. The crossover is carried out by swapping each and every bit between the two mated chromosomes, from the nth place to the l +. Implementing a so-called sign rule into this inbreeding refines each generation and, consequently, improves the convergence of the GA. Thus, half of the mated chromosomes preserve their sign character whereas the other half adopt the sign character of the fittest of the two mated chromosomes. Finally, any two mated chromosomes are taen out of the mating pool. Mutation After the crossover the mutation process wals through the string space i.e., all the strings within the population) and changes a bit from to and from to ) every N mut positions. Recommended values for N mut are rather large, thus this process plays only a secondary role in the GA. In this wor N mut was set equal to N η. Finally, encoding of the reproduced chromosomes yields a new generation of parameters. 3 of 24

14 D. The GA-QPF Algorithm Incorporating the GA into the QPF is carried out by first setting the times during which sequential buildup of the lielihood taes place. Recalling that the resampling stage worsens the posterior representation, thus typically yielding inferior estimates, it is reasonable to implement this stage whenever the resampling is not needed. This leads to executing the GA iteration every resampling. Thus, a single GA-QPF cycle is detailed in Algorithm 2 and illustrated in Fig.. Algorithm 2 The GA-QPF Require: Obtain previous step s QPF estimate, ˆq : if new measurement, Y, is available then Ensure: QPF: measurement update and get current estimate, ˆq 2: if N eff < N th then Ensure: QPF: Resample 3: Update ϕ Υ i)) using Eq. 57) and normalize using Eq. 58), for all i =,..., N η 4: Derive the bias estimate by computing the weighted average of all parameters in the population ˆΥ = N η i= ϕ Υ i))υ i) 5: Apply reproduction +perturbation), crossover and mutation to obtain a new set {Υ i), ϕ Υ i))} N η i= 6: else {Sequential buildup of the lielihood} 7: Set Υ i) = Υ i) for all i =... N η 8: For all {Υ i), ϕ Υ i))} N η i=, compute the fitness function using Eq. 57) and normalize using Eq. 58) 9: Derive the bias estimate by computing the weighted average of all parameters in the population ˆΥ = N η i= ϕ Υ i))υ i) : end if : end if 2: Use the derived estimate in the next time-step by subtracting it from the gyro angular velocity measurement ω + E. QPF Initialization Large initial attitude errors require a large number of quaternion particles, at least until the zones of high lielihood are populated. A simple initialization procedure that demands a significantly smaller number of particles is used in this wor. The idea is based on the fact that the first vector observation defines the quaternion of rotation up to one degree of freedom. This degree of freedom is used to generate the initial set of particles from the first observation Y = [b ; r ]. This technique is detailed in Algorithm 3. Algorithm 3 QPF Initialization : Set z an integer number so that N/z is also integer 2: Set β = 2π/N/z), β =, i = 3: for m =,..., N/z do 4: for j =,..., z do 5: Draw ζ N 3, covδb )), ζ 2 N, β 2 ) 6: b = b + ζ, β = 2 arccos bt r / b r )) 7: e = [ b r ) T / b r sin β cos β ] T 8: δe = [ bt / b sin 2 ζ 2 + β) cos 2 ζ 2 + β) ] T 9: q i) = δe e : i=i+ : end for 2: β = β + β 3: end for 4 of 24

15 ˆq ω Y Measurement Update q i) Quaternion Time Propagation Φω ˆΥ ) Measurement Update.. py q i)) N eff < N th Resampling q i) Υ j). Seq. Lielihood Buildup ϕ Υ j)) N eff < N th GA Υ j) z z ˆq ˆΥ Figure. GA-QPF Scheme VI. Cramer-Rao Lower Bound In this wor, the performance of the QPF is compared to the posterior discrete-time Cramer-Rao lower bound CRLB). Due to the complex computations that follow, the ensuing development is constrained to the case of unbiased gyro measurements. Denoting by ˆq any unbiased estimator of q, it is well nown from estimation theory that the estimation error covariance is bounded from below by E [ˆq q )ˆq q )] J 65) where J is the Fisher information matrix for measurements, defined as [ ] J = E q q log p q,y q, Y ) where y x = y T x is the Laplacian operator. It has been recently shown that the posterior information matrix can be computed recursively as where J + = D 22 66) D 2T J + D ) D 2 67) [ ] D =E q q + log p q+ q q + q ) 68a) D 2 =E [ q + q log p q+ q q + q ) ] 68b) D 22 =E [ q + q + log p q+ q q + q ) ] + E [ q + q + log p y+ q + y + q + ) ] The expectations in Eq. 67) are performed with respect to the complete joint probability space, namely the joint density of q, ɛ, y. For some arbitrary function F, the expectation is, therefore, evaluated as E [F q, ɛ, y )] = c) fq, E, Y )p q,ɛ,y q, E, Y )dy de dq 69) 5 of 24

16 A. CRLB for the Quaternion Estimation Problem From Eq. 67) it is clear that the CRLB recursion depends mostly upon the transition density p q+ q. However due to the state dependency in the quaternion vector, this transition density is singular. Thus, the CRLB is computed for the quaternion s vector part only. The density p ϱ+ ϱ is expressed using the process noise density function. Assuming a zero mean Gaussian white process noise, namely ɛ N 3, R ɛ ), the following expression is obtained log p ϱ+ ϱ ρ + ρ ) = log C 2 f ρ +, ρ ) T Rɛ f ρ +, ρ ) log det ɛ f ρ, ɛ )) 7) Assuming Gaussian white observation noise, namely δb N, R b ), it can be shown that the log-lielihood function and its gradient with respect to ϱ + are log p y+ ϱ + Y + ρ + ) = log C [ ] T [ ] b+ Aρ + )r + R b b+ Aρ + )r + 2 [ ] T [ ] ϱ+ log p y+ ϱ + Y + ρ + ) = ϱ+ Aρ+ )r + R b b+ Aρ + )r + 7a) 7b) where Aρ + ) denotes the attitude matrix corresponding to the quaternion with ρ + as its vector component. The next step for obtaining the CRLB consists of derivation of the Hessians in Eq. 67). The highly complicated analytic expressions for these matrices were obtained using the Maple symbolic algebra pacage and are omitted here for conciseness. VII. Simulation Study A simulation study has been performed to evaluate the performance of the new algorithm and to compare it to standard EKFs and to the recently proposed USQUE filter of Ref. 6. To cases were investigated. In the first case, the filters were run in a synthetic noise example, taen from Ref. 6. In the second case, real measurements from the Technion s TechSAT satellite were used to examine the performance of the algorithms in a non-gaussian measurement noise case. A. Synthetic Noise Case The GA-QPF was applied to a realistic spacecraft model and compared to the additive quaternion extended Kalman filter AEKF) of Ref. 3, to the multiplicative quaternion extended Kalman filter MEKF) of Ref. 2 and to the unscented quaternion estimator USQUE) recently presented in Ref. 6. In this example, the spacecraft is an Earth-pointing, near-circular 9 min orbit with an inclination of 35 deg. The spacecraft is equipped with a TAM and gyroscopic rate sensors. The TAM s noise is modeled as a zero-mean Gaussian white process with a standard deviation of 5 nt. The rate-integrating gyros RIG) output is contaminated with a measurement noise having two components: a white, zero-mean Gaussian process with intensity. µrad) 2 /s, and a drift bias modeled as an integrated Gaussian white noise with intensity 7 µrad) 2 /s 3. The RIGs initial bias is set to. deg/h on each axis. The Earth magnetic field is modeled using the 8th-order international Geomagnetic reference field IGRF). The initial attitude errors are set to -5 deg, 5 deg and 6 deg for the three axes, respectively norm attitude error of 2 deg). The initial attitude covariance of the USQUE filter is set to 5 deg 2 I 3 3 which seemed to be the most successful choice based on tuning runs). The GA-QPF is initialized with N = 5 particles and N η = 2 bias parameters, using the initialization scheme previously described. The initial bias parameters were simulated using a zero mean Gaussian ernel with the same covariance as in the Kalman filter variants and the chromosome length was set to l = 3 bit. The crossover procedure of the GA was applied to only 2 percent of the chromosomes and a perturbation bandwidth of h η =.3 was used. After 2 measurement updates, the filter continues using only N = 5 unit quaternion particles those corresponding to the largest importance weights). In all simulations, the particles were weighted using a scaled covariance 3 R b with R b = 5 2 I 3 3 nt) 2, and thus the lielihood function was { p y q Y q ) = exp } 2π) 3/2 3R b /2 2 b Aq )r ) T 3R b ) b Aq )r ) 72) 6 of 24

17 The resampling threshold is set to N th = 2 3N. The sampling rate of the TAM is per s, whereas the gyros are sampled at Hz. The attitude estimation errors in degrees) are computed as α = 2 arccosδq 4 ) 73) where δq 4 is the scalar component of the error quaternion δq. Figures 2 and 3 present the results of a -run Monte Carlo study and a typical single run, respectively. As can be see from these figures, the GA-QPF reaches its mean attitude estimation errors of less than. deg in about 5 minutes, whereas the USQUE filter taes about hours to reach the same accuracy level. 2.8 GA QPF USQUE AEKF MEKF.6.4 Quaternion est. error Deg) Time Hour) Figure 2. Mean quaternion estimation error for the GA-QPF, USQUE, AEKF and MEKF filters. Monte Carlo runs, synthetic noise case. The innovations processes of both the GA-QPF filter and the USQUE filter are shown in Fig. 4. It can be clearly seen that the GA-QPF filter processes better the measurements. The norms of the gyro bias errors of the GA-QPF and the USQUE are shown in Fig. 5, which demonstrates the benefit of the decoupling between the quaternion and the bias estimators. Starting with the same initial bias errors, the USQUE estimation errors begin to increase dramatically and then show some explicit dynamic behavior. These estimates of the bias are then used in the time propagation of the quaternion, and, consequently, lead to worse quaternion estimates. On the other hand, the GA-QPF bias estimation suffers from no such phenomenon and its bias estimation error decreases monotonically. B. TechSAT Noise Data The GA-QPF was also tested using a realistic TAM noise model of the Technion s TechSAT-Gurwin microsatellite. The TechSAT orbit is inclined at 98 deg and its period is min. As in the previous case, the satellite performs an Earth-pointing mission. Analyzing 75 hours of TAM data acquired once per s) allows estimating the TAM measurement noise joint PDF. This PDF is shown sliced) in Figure 6, and its marginals are shown in Figure 7. Clearly, these figures show that the TechSAT TAM s joint PDF is non-gaussian. This double peaed distribution is due to additional magnetic dipole moment along the Y body frame axis which was encountered during the momentum wheel slow down. The results for this case were obtained by simulating the TAM noise via sampling from the real noise data. In all runs, the initial attitude errors and bias were taen as in the previous example. The lielihood 7 of 24