Unscented Filtering for Spacecraft Attitude Estimation

A new spacecraft attitude estimation approach based on the Unscented Filter is derived. For nonlinear systems the Unscented Filter uses a carefully selected set of sample points to more accurately map the probability distribution than the linearization of the standard Extended Kalman Filter, leading to faster convergence from inaccurate initial conditions in attitude estimation problems. The filter formulation is based on standard attitude-vector measurements using a gyro-based model for attitude propagation. The global attitude parameterization is given by a quaternion, while a generalized three-dimensional attitude representation is used to define the local attitude error. A multiplicative quaternion-error approach is derived from the local attitude error, which guarantees that quaternion normalization is maintained in the filter. Simulation results indicate that the Unscented Filter is more robust than the Extended Kalman Filter under realistic initial attitude-error conditions.  The Extended Kalman Filter (EKF) is widely used in attitude estimation. Several parameterizations can be used to represent the attitude, such as Euler angles, quaternions, modified Rodrigues parameters, and even the rotation vector. Quaternions are especially appealing since no singularities are present and the kinematics equation is bilinear. However, ∗Associate Professor, Department of Mechanical & Aerospace Engineering, University at Buffalo, State University of New York, Amherst, NY 14260-4400. Associate Fellow AIAA.  Engineer, Guidance, Navigation and Control Systems Engineering Branch, NASA-Goddard Space Flight Center, Greenbelt, MD 20771. Fellow AIAA. J. L. Crassidis and F. L. Markley 1 of 34 the quaternion must obey a normalization constraint, which can be violated by the linear measurement-updates associated with the standard EKF approach. The most common approach to overcome this shortfall involves using a multiplicative error quaternion, where after neglecting higher-order terms the four-component quaternion can effectively be replaced by a three-component error vector. Under ideal circumstances, such as small attitude errors, this approach works extremely well. One interesting fact of the formulation presented in Ref.  is that the 4× 4 quaternion covariance is assumed to have rank three, i.e., the 4×4 matrix can be projected onto a 3×3 matrix without any loss in information. But, this is only strictly valid when the constraint is linear, which is not the case for the quaternion. However, the covariance is nearly singular, and a linear computation such as the EKF can make it exactly singular. This approach is justifiable for small estimation errors, but may cause difficulties outside the valid linear region, e.g., during the initialization stage of the EKF. Several approaches have addressed the issue of initialization for attitude estimation. Reference  explicitly includes the quaternion constraint in the measurement update. This works well for large initial condition errors; however, linearizations about the previous error estimate are required for the covariance propagation and for the measurement updates, which may produce biased estimates. Reference [7] breaks the measurement update into two steps. The linear first step uses a transformation of the desired states, while the second step uses a non-recursive minimization to recover the desired states. This also works well; however, local iterations at each time-step on a constrained minimization problem are required in the second step. In this paper a new attitude estimation approach, based on a filter developed by Julier, Uhlmann and Durrant-Whyte, is shown as an alternative to the EKF. This filter approach, which they call the Unscented Filter 9 (UF), has several advantages over the EKF, including: 1) the expected error is lower than the EKF, 2) the new filter can be applied to nondifferentiable functions, 3) the new filter avoids the derivation of Jacobian matrices, and 4) J. L. Crassidis and F. L. Markley 2 of 34 the new filter is valid to higher-order expansions than the standard EKF. The UF works on the premise that with a fixed number of parameters it should be easier to approximate a Gaussian distribution than to approximate an arbitrary nonlinear function. Also, the UF uses the standard Kalman form in the post-update, but uses a different propagation of the covariance and pre-measurement update with no local iterations. As with the standard EKF, using the UF directly with a quaternion parameterization of the attitude yields a nonunit quaternion estimate. A linearized model, such as the one used in Ref. , does not take full advantage of the UF capabilities. An alternative approach presented in this paper uses a three-component attitude-error vector to represent the quaternion error vector. Several three-component representations are possible, including the Gibbs vector (also known as the vector of Rodrigues parameters), which has a singularity at 180◦ rotations, and the vector of modified Rodrigues parameters (MRPs), which has a singularity at 360◦.10 Reference [11] proves that four times the vector of MRPs and twice the Gibbs vector leads to the same second-order approximation. We choose a generalized representation where the singularity can be placed anywhere from 180◦ to 360◦.12 Since we only use a three-component representation for the attitude errors, the singularity is never encountered in practice. The updates are performed using quaternion multiplication, leading to a natural way of maintaining the normalization constraint. The organization of this paper proceeds as follows. First, the UF is reviewed. Then, a brief review of the quaternion attitude kinematics and gyro-model is provided, followed by a review of the generalized three-component error-vector representation. Then, an UF is derived for attitude estimation using a multiplicative quaternion update with the threecomponent error-vector representation. Finally, the new filter is compared with the EKF using simulated three-axis magnetometer and gyro measurements of the Tropical Rainfall Measurement Mission (TRMM). J. L. Crassidis and F. L. Markley 3 of 34 Unscented Filtering In this section the UF is reviewed. The filter presented in Ref.  is derived for discretetime nonlinear equations, where the system model is given by xk+1 = f(xk, k) + Gkwk (1a) ỹk = h(xk, k) + vk (1b) where xk is the n × 1 state vector and ỹk is the m × 1 measurement vector. Note that a continuous-time model can always be expressed in the form of Eq. (1a) through an appropriate numerical integration scheme. We assume that the process noise wk and measurementerror noise vk are zero-mean Gaussian noise processes with covariances given by Qk and Rk, respectively. The standard Kalman Filter update equations are first rewritten as x̂+k = x̂ − k + Kkυk (2a) P k = P − k −KkP υυ k K k (2b) where x̂k and P − k are the pre-update state estimate and covariance, respectively, and x̂ + k and P k are the post-update state estimate and covariance, respectively. The innovation υk is given by υk ≡ ỹk − ŷ− k = ỹk − h(x̂k , k) (3) The covariance of υk is denoted by P υυ k . The gain Kk is computed by Kk = P xy k (P υυ k ) −1 (4) where P xy k is the cross-correlation matrix between x̂ − k and ŷ − k . The UF uses a different propagation than the standard EKF. Given an n× n covariance matrix P , a set of 2n sigma points can be generated from the columns of the matrices J. L. Crassidis and F. L. Markley 4 of 34 ± √ (n + λ)P , where √ M is shorthand notation for a matrix Z such that Z Z = M . The set of points is zero-mean, but if the distribution has mean μ, then simply adding μ to each of the points yields a symmetric set of 2n points having the desired mean and covariance. Due to the symmetric nature of this set, its odd central moments are zero, so its first three moments are the same as the original Gaussian distribution. The scalar λ is a convenient parameter for exploiting knowledge (if available) about the higher moments of the given distribution. In scalar systems (i.e., for n = 1), a value of λ = 2 leads to errors in the mean and variance that are sixth order. For higher-dimensional systems choosing λ = 3− n minimizes the mean-squared-error up to the fourth order. However, caution should be exercised when λ is negative since a possibility exists that the predicted covariance can become non-positive semi-definite. If this is a major concern, then another approach can be used that allows for scaling of the sigma points, which guarantees a positive semi-definite covariance matrix. Also, it can be shown that when n + λ tends to zero the mean tends to that calculated by the truncated second-order filter. This is the foundation for the UF. A method for incorporating process noise in the UF is shown in Ref. . This approach generates a set of points in [xk, wk] space that has the correct mean and covariance, and propagates these points through the model in Eq. (1a). The predicted mean and covariance are also augmented to included the process noise, but the basic structure of the their calculations remain unchanged   for more details). Although this approach may more fully utilize the capability of the unscented transformation, it will be more computationally costly due to the extra required calculations arising from the augmented system. For the attitude estimation problem, a set of six more sigma points is required to implement this approach. This significantly increases the computational burden, which may prohibit its use for actual onboard implementations. Another approach based on a trapezoidal approximation will be shown in this paper. The general formulation for the propagation equations is given as follows. First, compute J. L. Crassidis and F. L. Markley 5 of 34 the following set of sigma points: σk ← 2n columns from ± √ (n + λ)[P k + Q̄k] (5a) χk(0) = x̂ + k (5b) χk(i) = σk(i) + x̂ + k (