Table of Contents Author Guidelines Submit a Manuscript
Letter to the Editor
Mathematical Problems in Engineering
Volume 2009 (2009), Article ID 507370, 23 pages
http://dx.doi.org/10.1155/2009/507370
Research Article

Highly Efficient Sigma Point Filter for Spacecraft Attitude and Rate Estimation

1Department of Precision Instruments and Mechanology, Tsinghua University, Beijing 100084, China
2State Key Laboratory of Precision Measurement Technology and Instruments, Tsinghua University, Beijing 100084, China

Received 9 July 2009; Accepted 30 September 2009

Academic Editor: Tadashi Yokoyama

Copyright © 2009 Chunshi Fan and Zheng You. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

Abstract

Nonlinearities in spacecraft attitude determination problem has been studied intensively during the past decades. Traditionally, multiplicative extended Kalman filter_MEKF_algorithm has been a good solution for most nominal space missions. But in recent years, advances in space missions deserve a revisit of the issue. Though there exist a variety of advanced nonlinear filtering algorithms, most of them are prohibited for actual onboard implementation because of their overload computational complexity. In this paper, we address this difficulty by developing a new algorithm framework based on the marginal filtering principle, which requires only 4 sigma points to give a complete 6-state attitude and angular rate estimation. Moreover, a new strategy for sigma point construction is also developed to further increase the efficiency and numerical accuracy. Incorporating the presented framework and novel sigma points, we proposed a new, nonlinear attitude and rate estimator, namely, the Marginal Geometric Sigma Point Filter. The new algorithm is of the same precision as traditional unscented Kalman filters, while keeping a significantly lower computational complexity, even when compared to the reduced sigma point algorithms. In fact, it has truly rivaled the efficiency of MEKF, even when simple closed-form solutions are involved in the latter.

1. Introduction

Nonlinearities in spacecraft attitude determination problem have been studied intensively during the past decades. Since the early 1980’s, multiplicative extended Kalman filtering (MEKF) algorithm [1] has proven to be a successful solution for engineering application. The MEKF algorithm has a very low computing cost and performs quite well in most nominal space missions where the spacecraft’s angular rate is slow and the nonlinearities are not so impactive.

In recent years, advances in space missions, such as the greater agility demand and the application of lower cost sensors, deserve a revisit of the nonlinearity issue. Although a variety of advanced nonlinear filtering algorithms exist, only few of them are close to the restrict numerical expense requirements of actual onboard implementations. In the existing methods, the well-known sigma point Kalman filters (SPKFs) [2, 3] have approven to be among the most efficient ones. SPKF bases on a Gaussian distribution approximation technique, namely, the unscented transformation (UT), where a deterministic set of weighted points (known as the sigma points) are used to make probabilistic inference [4]. Eliminating the complex Jacobian matrix derivations, SPKF algorithms are much easier to implement and have better performance than traditional widely used EKF algorithms; so they have found widespread application in a variety of fields. In recent years, spacecraft attitude estimation problems have also been addressed by SPKF approaches in literature and engineering practice [57].

In spite of being efficient among nonlinear filters, baseline SPKF still seems computational costly for engineering implementation. If we denote 𝑚 as the number of sigma points required, then for an 𝑛-dimension random state vector 𝐱, standard unscented transformation needs a set of 𝑚=2𝑛+1 points to capture the state’s statistical distribution properties. More seriously, when we develop a complete SPKF estimator for the n-dimensional state model, the actual 𝑚 needed is no longer 2𝑛+1—it easily becomes 4n or even larger, because standard SPKF algorithm requires a state augmentation to include all the propagation and measurement noise terms, hence leading to an unacceptable increase in computational burden. For avoidance of state augment, iterated and trapezoidal approximation approaches [5, 8] have been developed. Both approaches are suitable for additive noise case only, and they are able to reduce 𝑚 back to 2𝑛+1. To further reduce the complexity, strategies for introducing fewer sigma points are exploited, known as the reduced sigma point filters. Several simplex points selection strategy have been developed, including the 𝑛+2 point minimal-skew simplex points [9], the spherical simplex points [10, 11], and some enhanced higher-order extensions [12]. Each of the above sigma point sets involves a zero-valued “central point,” which is usually endowed with a negative weight so as to minimize higher order effects, known as the scaled UT technique [13]. In recent years, new sigma point selection strategy involving no central-point is introduced, such as the Schmidt orthogonalization-based simplex set [14], which includes 𝑛+1 equally weighted points. It is also proved that [15] such negative weight-free, equally weighted sigma point sets are numerically more stable and accurate as well as having a better efficiency. In fact, both central-point elimination and equal weight assignment improve the symmetry property of the sigma point set, and a better symmetry property provides a better numerical behavior, as it has a better capability to suppress the impact of the round off errors. In this article, we address the construction strategies to make a best symmetric structure in simplex sigma point set.

Anyhow, applications of simplex sets have reduced the required sigma points to 50% of the traditional nonaugmented algorithms and have made a significant improvement in numerical efficiency. In fact, the numerical efficiency of simplex SPKF algorithm is able to rival or even exceed its EKF counterpart if general formed Riccati equations and numerical integration process are involved in the latter. However, for typical quaternion-based spacecraft attitude problems, since simple analytical solutions and sparse matrices exist for MEKF’ covariance propagation and measurement updates, general simplex SPKF algorithm still compares unfavorably with MEKF in efficiency.

Clearly, to develop a competitive algorithm alternative to the MEKF for practical applications, we need a still further reduction of 𝑚. Recently, a new sigma point selection strategy for a class of “partially linear” transformation problems has been proposed, namely, the marginal unscented transformation [16]. By exploiting the linear substructures in system model and margining out the corresponding variables, the marginal UT algorithms only needs a set of sigma points that adequately describes the statistics property of the nonlinear part of the states. It provides a possible approach to further shrink the size of the sigma point set. As attitude dynamics also has linear substructures in gyro bias drift model and the observation equations, it is imaginable that we can also address the attitude and rate estimator design with similar ideas.

The main contributions of this article include two parts. First, we have derived in detail a marginal version of SPKF algorithm for typical 6-state attitude determination system. The new algorithm uses merely 4 sigma points to give a complete attitude and angular rate estimation; hence it is able to achieve a high numerical efficiency that truly rivals the MEKF. Second, we have proposed a new set of simplex sigma points for Euclidean Geometric space, named the Geometric Simplex sigma point set. The new set has a symmetric structure, a lower computational expense and is numerically more accurate. It would be of use in a variety of 3-dimensional modeled dynamic problems.

The organization of this paper proceeded as follows. First, we established a general 6-state stellar-inertial spacecraft attitude kinemics and measurement model and analyzed the partially linear structure in the system. Then a marginal SPKF estimator is derived in detail. Next, we looked into the asymmetrical properties of existing sigma point construction algorithms and proposed the Geometric simplex set. Finally, we incorporated the proposed sigma point set into the marginal filtering framework to configure a complete attitude estimator, named the Marginal Geometric Sigma Point Filter, and inspected its performance in simulation with comparisons to the traditional MEKF and Spherical Simplex SPKF.

2. Models and Traditional Filtering Frameworks

2.1. Attitude Dynamics and the MEKF Framework

For spacecraft attitude estimation, quaternion has been the most widely used attitude parameterization. The quaternion is given by a 4-dimension vector defined as 𝐪=[𝐪𝑇,𝑞4]𝑇, with 𝐪[𝑞1,𝑞2,𝑞3]𝑇=̂𝐧sin(𝜙/2) and 𝑞4=cos(𝜙/2), where ̂𝐧 is Euler axis and 𝜙 is the rotation angle. Quaternion parameter satisfies a single constraint given by 𝐪𝑇𝐪=1. The kinematics equation is given by

̇1𝐪=2𝛀(𝝎)𝐪,(2.1) where 𝝎 is the angular rate vector given from the gyro’s measurement 𝝎meas by compensating the gyro bias 𝐛:

𝝎=𝝎meas𝐛𝜼ARW,(2.2) where 𝜼ARW is a zero-mean Gaussian angular random walk noise with a covariance of 𝜎2ARW𝐈3. 𝐛 is often modeled as a rate random walk process with white noise 𝜼RRW and a covariance of 𝜎2RRW𝐈3:

̇𝐛=𝜼RRW.(2.3)

From (2.1)(2.3), we can derive the discrete-time version of the above models with numerical integration. In fact, numerically simpler closed-form solution exists for (2.1) if we approximately consider the direction of 𝝎 as a constant vector during the propagation interval 𝑇 for each filtering circle from 𝑡𝑘1 to 𝑡𝑘:

̂𝐪𝑘/𝑘1=̂𝐪𝑘1̂𝐪𝝎𝑘1,(2.4) where

̂𝐪𝝎𝑘1=𝝓𝑇𝑘12𝜙sin𝑘1/2𝜙cos𝑘1𝜙/2,cos𝑘12𝑇(2.5) with

𝝎𝑘1=𝝎meas𝑘1̂𝐛𝑘1,𝝓𝑘1=𝝎𝑘1𝜙𝑇,𝑘1=|||𝝓𝑘1|||.(2.6) The approximation is tenable as far as the period T is small enough, which is usually well satisfied in practice, and a second-order accuracy is guaranteed.

In actual calculation process, (2.5) seems too complex, and a 2nd-order approximation is enough. To associate the quaternion ̂𝐧𝐪(𝝓)=[𝑇sin(𝜙/2),cos(𝜙/2)]𝑇 to its pertinent rotation vector ̂𝝓=𝐧𝜙 in a straightforward way, we can choose an arbitrarily 3-dimensional attitude parameter as the media. In this paper, we choose the Modified Rodrigues Parameters (MRPs) recommended in [17] to give a 2nd-order, trigonometric function, and square-root function-free approximation of (2.5):

𝝓(𝛿𝐪)=4𝛿𝐪1+𝛿𝑞4,𝛿𝐪(𝝓)=8𝝓𝑇,16𝜙2𝑇16+𝜙2(2.7) with 𝜙2=𝝓𝑇𝝓. Clearly, such MRP-based expression is quite economic for computation. Then the discrete-time propagation equations can be written as follows:

̂𝐪𝝎𝑘1=8𝝓𝑇𝑘1,𝜙162𝑘1𝑇𝜙16+2𝑘1,̂𝐛𝑘/𝑘1=̂𝐛𝑘1.(2.8)

To cope with the unit-length constraint of quaternion, local error states (also known as the local disturbance states) are introduced into filter design. We describe the local attitude error as a 3-dimensional rotation vector 𝐚 and the local gyro bias error as another 3-dimensional vector Δ𝐛. Then the actual state vector processed by the filter is the 6-dimension disturbance state:

𝐚𝐱=𝑇,Δ𝐛𝑇𝑇.(2.9) For clarity, we hence address the original states ̂𝐪 and 𝐛 as the “global states,” which would mainly serve as singular-point-free reference and storage for the filter. The global states and the local error state are affiliated as

̂𝐪=𝐪𝛿̂̂𝐪(𝐚),𝐛=𝐛+Δ𝐛,(2.10) where 𝛿𝐪 is the local error quaternion corresponding to ̂𝐚. Again we choose to use the MRP approximation, as

𝐚(𝛿𝐪)=4𝛿𝐪1+𝛿𝑞4,𝛿𝐪(𝐚)=8𝐚𝑇,16𝑎2𝑇16+𝑎2(2.11) with 𝑎2=𝐚𝑇𝐚. Clearly, such an MRPs-based expression is free from any square-root or trigonometric functions, economic in computation.

After all generality, the observation model in this article is established as an automatic star sensor with quaternion measurements 𝐪meas𝑘. But in actual practice this information is presented to the Kalman filter in a more convenient way as in terms of a 3-dimensional parameter. Again we choose to use the MRP parameter, and then the star sensor’s observation model is simply defined as the local error between the predicted and observed attitudes:

𝐳meas𝑘̂𝐱=𝐡𝑘𝛿=𝐚𝐪meas𝑘+𝐯𝑘,(2.12) where 𝛿𝐪meas𝑘=̂𝐪1𝑘/𝑘1𝐪meas𝑘,(2.13) and 𝐯𝑘 is the measurement noise covariance modeled as

𝐑𝑘=𝜎2𝑟𝐈3.(2.14) With the above models, we can derive an MEKF estimator as in [1] and briefly reviewed in blocked matrix form in Table 4. Note that the most computational cost parts of the algorithm involve the covariance propagation, measurement update, and the Kalman filter gain computing, namely,

𝐏𝑋𝑘/𝑘1=𝚯𝑘1𝐏𝑋𝑘1𝚯𝑇𝑘1+𝐐𝑘𝐊,(2.15)𝑘=𝐏𝑋𝑘/𝑘1𝐇𝑇𝑘𝐇𝑘𝐏𝑋𝑘/𝑘1𝐇𝑇𝑘+𝐑𝑘1,𝐏(2.16)𝑋𝑘=𝐈6𝐊𝑘𝐇𝑘𝐏𝑋𝑘/𝑘1,(2.17) where 𝐏𝑋𝑘 is the estimation of 𝐱’s covariance, 𝐊𝑘 is the Kalman filter gain, 𝐇𝑘 is the observation matrix, and Θ𝑘 and 𝐐𝑘 are, respectively, the transition matrix and the propagation noise matrix, as

𝚯𝑘1=𝚽𝑘1𝚿𝑘1𝟎3×3𝐈3,𝐐𝑘=𝐐𝐴𝑘𝐐𝑘𝐵𝐴𝑇𝐐𝑘𝐵𝐴𝐐𝐵𝑘.(2.18)

General problem would involve numerical integration of Riccati equations to evaluate the Θ𝑘 matrix in (2.15), and complex derivations to evaluate the 𝐇𝑘 matrix in (2.16) and (2.17). Fortunately, it is also found that simple analytical solutions exist for Φ𝑘1, Ψ𝑘1 (see Table 4) [18], and 𝐐𝑘 [19] as 𝐐𝐴𝑘𝜎=𝑇2ARW+13𝜎2RRW𝑇2𝐈3,𝐐𝑘𝐵𝐴1=2𝜎2RRW𝑇2𝐈3,𝐐𝐵𝑘=𝜎2RRW𝑇𝐈3.(2.19) Moreover, 𝐇𝑘 involves sparse and unit matrices as

𝐇𝑘=𝐈3,𝟎3×3.(2.20) Hence the MEKF algorithm is numerically very efficient. The computational complex of a typical stellar-inertial MEKF attitude estimator is evaluated in Table 4.

2.2. Traditional Sigma Point Filtering Framework

We consider now the application of SPKF to the system discussed above. A set of 𝑚 sigma points are chosen to approximate the statistic distribution of the 6-dimension disturbance state 𝐱=[𝐚𝑇,Δ𝐛𝑇]𝑇:

𝝌𝜶(𝑖)=(𝑖)𝑇,𝜷(𝑖)𝑇𝑇,𝑖=1,2,,𝑚,(2.21) where (𝑖) is the index, 𝜶(𝑖), 𝜷(𝑖) represent the attitude and bias error, respectively, each point is assigned with a weight 𝑊(𝑖), and all the weights satisfy the normalization constraint: 𝑚𝑖=1𝑊(𝑖)=1.(2.22) For clarity, write the sigma points and their associated weight in matrix form, as

𝝌𝒳=(1),𝝌(2),,𝝌(𝑚)=𝑎=𝜶(1),𝜶(2),,𝜶(𝑚)𝜷(1),𝜷(2),,𝜷(𝑚),𝑊𝒲=(1),𝑊(2),,𝑊(𝑚),𝚲𝒲=diag(𝒲).(2.23) For unbiased distribution of 𝐱𝑛, 𝒳 is constructed as

𝒳=𝐒𝑋𝒰,(2.24) where 𝐒𝑋 is an arbitrary square-root matrix of 𝐏𝑋 with 𝐒𝑋(𝐒𝑋)𝑇=𝐏𝑋=𝐜𝐨𝐯(𝐱,𝐱), also denoted as 𝐒𝑋=𝐏𝑋. 𝒰=[𝐮(1),,𝐮(𝑚)]𝑛×𝑚 is a base set of sigma points, and it can be defined in several different rules, depending on the sigma point construction strategy we use. Anyhow, 𝒰 has an unbiased mean and a unit covariance: 𝒰𝒲𝑇=𝟎,𝒰𝚲𝐖𝒰𝑇=𝐈𝑚.(2.25) The construction strategy of 𝒰 is also the dominating differentiation between different SPKF algorithms. With the definitions above, the set 𝐗 is able to capture the statistics of 𝐱’s distribution precisely up to 2nd-order.

The SPKF attitude estimator is as follows. At the beginning time of each filtering step 𝑡𝑘1, the local error state is reset to zero:

̂𝐱𝑘1=̂𝐚𝑇𝑘1̂𝐛,Δ𝑇𝑘1𝑇=𝟎3×1,𝟎3×1𝑇.(2.26) Then we construct the sigma points of 𝐗𝑘1. As in [5], we would like to use the trapezoidal approximation to avoid state augmentation; so actually 𝐒𝑋𝑘1 is computed as

𝐒𝑋𝑘1=𝐏𝑋𝑘1+𝐐𝑋𝑘1,(2.27) where

𝐐𝑋𝑘1=𝐐𝐴𝑘1,𝟎3×3𝟎3×3,𝐐𝐵𝑘1=𝑇2𝜎2ARW𝑇26𝜎2RRW𝐈3,𝟎3×3𝟎3×3,𝑇2𝜎2RRW𝐈3,(2.28) and it is equivalent to have the 𝐐𝑘 in (2.18) implicitly propagated together with the sigma points [5].

Next, propagate the 𝑚 sigma points as follows:

𝝓𝜷(𝑖)𝑘1=𝝎𝑘1𝜷(𝑖)𝑘1𝑇,𝐪𝜷(𝑖)𝑘1=8𝝓𝜷(𝑖)𝑇𝑘1,16𝜙𝜷(𝑖)2𝑘1𝑇16+𝜙𝜷(𝑖)2𝑘1,(2.29) where 𝜙𝜷(𝑖)𝑘1=|𝝓𝜷(𝑖)𝑘1|, and 𝐪𝜶(𝑖)𝑘1=8𝜶(𝑖)𝑇𝑘1,16𝛼(𝑖)2𝑘1𝑇16+𝛼(𝑖)2𝑘1,𝐪𝜶(𝑖)𝑘/𝑘1=̂𝐪𝝎𝑘11𝐪𝜶(𝑖)𝑘1𝐪𝜷(𝑖)𝑘1.(2.30) Thereby we have the propagated sigma points 𝑎𝑘/𝑘1 and 𝑘/𝑘1, respectively, as 𝜶(𝑖)𝑘/𝑘1=𝜶𝐪𝜶(𝑖)𝑘/𝑘1=4𝐪𝜶(𝑖)𝑘/𝑘11+𝑞𝜶(𝑖)4𝑘/𝑘1,𝜷(2.31)(𝑖)𝑘/𝑘1=𝜷(𝑖)𝑘1.(2.32) Note we have propagated the attitude-related sigma points 𝜶(𝑖) directly without adhering it to the global state ̂𝐪𝑘1 as in [5]; so a decrease of computational complexity is achievable. This approach stands as far as the time interval of 𝑇 is guaranteed to be small enough in order that both 𝜶(𝑖) and 𝝓(𝑖) can be taken as small rotation vectors, and the MRP approximation is valid. Then the predicted mean of 𝐱 is ̂𝐱𝑘/𝑘1=𝒳𝑘/𝑘1𝒲𝑇.(2.33) Now we compute the predicted measurements. Noting that the sigma points have already implicitly changed their reference from ̂𝐪𝑘1 to ̂𝐪𝑘/𝑘1 during propagation, so the predicted star sensor quaternion measurements are

̂𝐪𝝌(𝑖)𝑘/𝑘1=̂𝐪𝑘/𝑘1𝛿𝐪𝜶(𝑖)𝑘/𝑘1.(2.34) Following (2.12) and (2.13), it is straight forward to derive the observation model for the SPKF as 𝜻(𝑖)𝑘/𝑘1=𝜶(𝑖)𝑘/𝑘1.(2.35) Denoted in matrix form as 𝒵=[𝜻(1),𝜻(2),,𝜻(𝑚)], then we can get the predicted measurement ̂𝑧𝑘/𝑘1 as

̂𝑧𝑘/𝑘1=𝖅𝑘/𝑘1𝖂𝑇.(2.36)

Next we compute the covariance predictions. If we take ̂𝐱𝑘/𝑘1 as a bias of the sigma points, then to appropriately compute the covariance, we must remove this bias at first. Hence the covariance prediction process would involve two steps:

𝒳𝑘/𝑘1=𝒳𝑘/𝑘1𝟏1×𝑚̂𝐱𝑘/𝑘1,𝐏(2.37)𝑋𝑘/𝑘1=𝖃𝑘/𝑘1𝚲𝖂𝖃𝑇𝑘/𝑘1,(2.38) where “” denotes the Kronecker product, and 𝟏1×𝑚=[1,1,,1]1×𝑚. Similarly, we give the unbiased measurement points set as

𝖅𝑘/𝑘1=𝖅𝑘/𝑘1𝟏1×𝑚̂𝑧𝑘/𝑘1(2.39) the innovation covariance and cross covariance matrix are then, respectively, 𝐏𝑍𝑘/𝑘1=𝖅𝑘/𝑘1𝚲𝒲𝖅𝑇𝑘/𝑘1+𝐑𝑘,𝐏𝑋𝑍𝑘/𝑘1=𝖃𝑘/𝑘1𝚲𝒲𝖅𝑇𝑘/𝑘1(2.40) and the measurement update procedure is

𝐊𝑘=𝐏𝑋𝑍𝑘/𝑘1𝐏𝑍𝑘/𝑘11,̂𝐱(2.41)𝑘=̂𝐱𝑘/𝑘1+𝐊𝑘̂𝑧meas𝑘̂𝑧𝑘/𝑘1𝐏,(2.42)𝑋𝑘=𝐏𝑋𝑘/𝑘1𝐊𝑘𝐏𝑍𝑘/𝑘1𝐊𝑘𝑇.(2.43) Finally, update the global states ̂𝐪 and ̂𝐛 as

̂𝐪𝑘=̂𝐪𝑘/𝑘1𝛿𝐪̂𝐚𝑘,̂𝐛𝑘=̂𝐛𝑘/𝑘1̂𝐛+Δ𝑘.(2.44)

Above is the framework of an SPKF version attitude estimator. A blocked-form procedure summary is listed in Table 5. It is not difficult to evaluate the computational complexity of the SPKF estimator. Even when reduced-point algorithms, such as the spherical simplex unscented Kalman filter, are adopted, the computing effort is still a double of the MEKFs, as listed in Table 3.

3. Marginal Geometric Sigma Point Filters

3.1. Marginal Sigma Point Filtering Framework for Spacecraft Attitude and Rate Estimations

Now we look into some special structures of the above estimator. First, noting (2.32), we find that the bias-related sigma points =[𝜷(1),,𝜷(𝑚)] remain unchanged during the whole process of the propagation, indicating that their mean would also remain unchanged as Δ̂𝐛𝑘/𝑘1̂𝐛=Δ𝑘1=𝟎3×1. In other words, no information has been introduced into the state Δ𝐛’s mean and covariance 𝐜𝐨𝐯(Δ𝐛,Δ𝐛) during the propagation. Therefore, once we are able to capture the information of ̂𝐚, 𝐜𝐨𝐯(𝐚,𝐚) and 𝐜𝐨𝐯(𝐛,𝐚), we have already obtained all the information available during time propagation.

Then noting (2.35), we find only the attitude-related sigma points 𝑎𝑘/𝑘1 are explicitly used to construct the measurement predictions 𝒵𝑘/𝑘1. We can write a formal expression of this transform as

𝑧=𝐡(𝐱)=𝜸(𝐚).(3.1) In fact, (3.1) belongs to a special class of nonlinear transformation, namely, the partially linear transformation [16]. Clearly, for the measurement update process, the random variable 𝑧’s mean 𝑧, 𝐜𝐨𝐯(𝑧,𝑧), and cross covariance 𝐜𝐨𝐯(𝐚,𝑧) are all independent of Δ𝐛, and it is proved that the cross covariance 𝐜𝐨𝐯(Δ𝐛,𝑧) is also independent of 𝐜𝐨𝐯(Δ𝐛,Δ𝐛) up to the 2nd-order.

The above discussions leads to the following conclusion: as long as we can construct a set of sigma points that matches the given mean estimations of ̂𝐚, Δ̂𝐛, and the covariances of 𝐏𝐴=𝐜𝐨𝐯(𝐚,𝐚) and 𝐏𝐵𝐴=𝐜𝐨𝐯(Δ𝐛,𝐚), it is enough to capture the first two moments’ statistics properties of the random state 𝐱 with a precision up to 2nd-order. Before moving on to construct such a sigma point set, we should also note from (2.26), (2.37) and (2.39) that with the reset and de-bias steps embeding the filter, actually we are on the assumption that we have an unbiased distribution as 𝐱=[𝐚𝑇,Δ𝐛𝑇]𝑇=[𝟎𝑇3×1,𝟎𝑇3×1]𝑇.

So now our goal becomes to construct a minimum set of sigma points, which is able to fully capture all the available information given as (a) unbiased mean estimation, that is, 𝐚=𝟎3×1, Δ𝐛=𝟎3×1, and (b) the covariance estimation as 𝐏𝐴=𝐜𝐨𝐯(𝐚,𝐚)𝐏𝐵𝐴=𝐜𝐨𝐯(Δ𝐛,𝐚).

To match the unbiased mean, we have

𝑎𝖂𝑇=𝑊𝑚𝜶(1)+𝜶(2)++𝜶(𝑚)=𝟎,(3.2)𝖂𝑇=𝑊𝑚𝜷(1)+𝜷(2)++𝜷(𝑚)=𝟎.(3.3) As stated in [9], to fully capture the mean of an n-dimensional state vector, at least 𝑚=𝑛+1 points are needed. Noting both 𝐚 and Δ𝐛3, the minimum 𝑚 which satisfies both (3.2) and (3.3) is 𝑚=4.

To further reduce the computational expense and make a better symmetry property, assign equal weights 𝑊 for all the sigma points. Then we have

𝑚𝑖=1𝑊𝑖=𝑚𝑊=1.(3.4) Clearly, 1𝑊=𝑚=14,1(3.5)𝖂=𝑚𝟏1×𝑚,𝚲𝖂=1𝑚𝐈𝑚.(3.6)

To match 𝐚’s covariance estimation 𝐏𝐴=𝐜𝐨𝐯(𝐚,𝐚) with outer products approximation, we have

𝑎𝚲𝖂(𝑎)𝑇=1𝑚𝑎(𝑎)𝑇=𝐏𝐴.(3.7) Again we make use of a base set 𝖀𝑎3×𝑚 to help matching 𝐏𝐴. Denoting 𝖀𝑎=[𝐮𝑎(1),𝐮𝑎(2),𝐮𝑎(3),𝐮𝑎(4)], the following relationship should be satisfied:

𝖀𝑎𝖂𝑇𝖀=𝟎,(3.8)𝑎𝚲𝖂𝖀𝑎𝑇=𝐈3.(3.9) Substituting (3.5) and (3.6), it is straightforward to get

𝑚𝑖=1𝐮𝑎(𝑖)𝖀=𝟎,(3.10)𝑎𝖀𝑎𝑇=𝑚𝐈3.(3.11) As 𝑚=4, 𝖀𝑎 must be a simplex set. In spite of the constraints in (3.8) and (3.9), we still have the freedom to choose an arbitrary realization of 𝖀𝑎. Later we will propose a novel set derived from a heuristic geometry approach. Now supposing that 𝖀𝑎 is given, then we may construct 𝑎 simply as 𝑎=𝐒𝐴𝖀𝑎,(3.12) where 𝐒𝐴 is an arbitrary matrix square-root of 𝐏𝐴 fulfilling 𝐒𝐴(𝐒𝐴)𝑇=𝐏𝐴. It is straightforward to validate (3.12) as substituting it to (3.7):

𝑎𝚲𝖂(𝑎)𝑇=1𝑚𝐒𝐴𝖀𝑎𝐒𝐴𝖀𝑎𝑇=1𝑚𝐒𝐴𝖀𝑎𝖀𝑎𝑇𝐒𝐴𝑇=1𝑚𝐒𝐴𝑚𝐈3𝐒𝐴𝑇=𝐏𝐴.(3.13) To match the cross covariance 𝐏𝐵𝐴 is

𝕭𝚲𝖂(𝑎)𝑇=1𝑚𝕭(𝑎)𝑇=𝐏𝐵𝐴.(3.14) Here we propose a simple and convenient algorithm to compute 𝕭. Define 𝐒𝐵𝐴𝑘1=𝐏𝐵𝐴(𝐒𝐴)𝑇, which we could get with the low computational cost Gaussian elimination:

𝐒𝐵𝐴𝑘1=𝐏𝐵𝐴𝐒𝐴𝑇=𝐏𝐵𝐴𝑘1𝐒𝐴𝑘1𝑇.(3.15) Then we have 𝕭=𝐒𝐵𝐴𝑘1𝖀𝑎.(3.16) Proof of (3.15) is straightforward as substituting it to (3.14):

𝕭𝚲𝒲(𝑎)𝑇=1𝑚𝐒𝐵𝐴𝑘1𝖀𝑎𝐒𝐴𝖀𝑎𝑇=1𝑚𝐒𝐵𝐴𝑘1𝖀𝑎𝖀𝑎𝑇𝐒𝐴𝑇=1𝑚𝐏𝐵𝐴𝐒𝐴𝑇𝑚𝐈3𝐒𝐴𝑇=𝐏𝐵𝐴𝐒𝐴𝑇𝐒𝐴𝑇=𝐏𝐵𝐴.(3.17) Thereupon, we have constructed a desired set of marginal sigma points as in (3.12) and (3.16). Note that it is enough to use merely one base set 𝖀𝑎 to construct the full-length Sigma point set 𝒳=[𝑎𝑇,𝕭𝑇]𝑇. So hereafter we would suppress the superscript of 𝖀𝑎 as 𝖀.

Looking into (2.38), we find

𝐏𝑋=𝐏𝐴,𝐏𝐴𝐵𝐏𝐵𝐴,𝐏𝐵=𝐏𝐴,𝐏𝐵𝐴𝑇𝐏𝐵𝐴,𝐏𝐵.(3.18) With the proposed sigma points, the propagation and innovation steps of Δ𝐛’s covariance estimation 𝐏𝐵 are no longer necessary. Eliminating the 𝐏𝐵-related term and making use of the symmetric structure of the matrix, it is enough to have 𝐏𝐴 and 𝐏𝐵𝐴 propagated. Accordingly, it is only necessary to have the same matrices updated. There by, we will replace (2.38) and (2.43) with

𝐏𝐴𝑘/𝑘1=𝑎𝑘/𝑘1𝚲𝖂𝑎𝑘/𝑘1𝑇,𝐏𝐵𝐴𝑘/𝑘1=𝕭𝑘/𝑘1𝚲𝖂𝑎𝑘/𝑘1𝑇,𝐏𝐴𝑘=𝐏𝐴𝑘/𝑘1𝐏𝐴𝑍𝑘/𝑘1𝐏𝑍𝑘/𝑘11𝐏𝐴𝑍𝑘/𝑘1𝑇=𝐏𝐴𝐴𝑘/𝑘1𝐊𝑘𝐴𝑍𝐏𝐴𝑍𝑘/𝑘1𝑇,𝐏𝑘𝐵𝐴=𝐏𝐵𝐴𝑘/𝑘1𝐏𝐵𝑍𝑘/𝑘1𝐏𝑍𝑘/𝑘11𝐏𝐴𝑍𝑘/𝑘1𝑇=𝐏𝐵𝐴𝑘/𝑘1𝐊𝑘𝐵𝑍𝐏𝐴𝑍𝑘/𝑘1𝑇.(3.19)

To avoid state augmentation, we would like to have the propagation noise terms incorporated into the filter with trapezoidal approximation. However, as 𝐏𝐵 is no longer used, we have to seek for alternate approach. Denote

𝐒𝑄𝐴𝑘1=𝐐𝐴𝑘1=𝑇22𝜎2ARW𝑇26𝜎2RRW𝐈3,𝐒𝑄𝐵𝑘1=𝐐𝐵𝑘1=𝜎RRW𝑇2𝐈3.(3.20) Then similar to [11], we can add the noise terms directly to the sigma points with the help of 𝖀:

𝑎𝑘1=𝐒𝐴𝑘1𝖀+𝐒𝑄𝐴𝑘1𝐒𝖀=𝐴𝑘1+𝐒𝑄𝐴𝑘1𝖀𝕭,(3.21)𝑘1=𝐒𝐵𝐴𝑘1𝖀𝐒𝐵𝑄𝐒𝖀=𝐵𝐴𝑘1𝐒𝐵𝑄𝖀.(3.22) Note in (3.22) that the sign before 𝐒𝐵𝑄 is negative. The reason is that with a given covariance estimation 𝐏 and its square-root 𝐒, it is equivalent for us to choose either 𝐒 or –𝐒 when constructing the sigma points. Both “directions” work because the given mean is unbiased. But when we add noise terms directly to the sigma points as in (3.22), we must guarantee that 𝐒𝐵𝐴𝑘1 and 𝐒𝐵𝑄 have a common sign. Recalling (25) as

𝐐𝐵𝐴𝑘11=2𝜎2RRW𝑇2𝐈3,(3.23) here the negative sign in the right side of (3.23) clearly indicates opposite signs between 𝐒𝐵𝐴𝑘1 and 𝐒𝐵𝑄.

3.2. The Geometric Simplex Sigma Points

Construction of the base set 𝖀 plays an important role in simplex sigma point algorithms. Existing strategies had mainly focused on the design of general operation flow for getting a base set for any arbitrary dimension 𝑛. In [9, 10], direction-extending technique is developed and used to build the minimal-skew and spherical simplex set. While in [14], Schmidt orthogonalization is employed to develop a new set. Table 2 demonstrates both the spherical simplex set [10] and the Schmidt orthogonal simplex set [14] for 𝑛=3. Close comparison could reveal that both sets are equivalent after a sign shift except for the existence of an additional central point in the spherical simplex set. Both sets are easy to be extended to higher dimension space, and because all the points are equally weighted and equidistantly placed on a hyper sphere, they are numerically stable over the increase of 𝑛.

However, both sets lack numerical accuracy, and they are complex to compute, as irrational numbers 2, 6, and so on exist. Further, they do not have symmetric structures. A fully symmetric set needs that for every point 𝐮(𝑖)𝖀, we can get another point 𝐮(𝑗)𝖀,𝑖𝑗 simply by element permutation or sign-changing point of the generator point 𝐮(𝑖) [15]. Clearly, except for the first dimension (or describing in matrix language, the first row of 𝖀), not even element level symmetry is guaranteed in the spherical or Schmidt orthogonal simplex sets.

In order to make a better symmetry property, we propose a new base set of sigma points here as in the 3rd row of Table 1 and Figure 1. It is straight forward to find that both (3.10) and (3.11) are completely satisfied. For clarity, we name this new set as “the Geometric Simplex Set,” for it has a nice symmetrical structure as a tetrahedron in the 3-dimensional Euclidean space (Figure 1). The proposed new sigma point set has several benefits.

tab1
Table 1: Simplex base sets for 3-dimensional space.
tab2
Table 2: Residues of numerical mean estimation.
tab3
Table 3: Comparisons of total arithmetic operations.
tab4
Table 4: Pseudocode and computational expense evaluation of the MEKF with closed-form solutions.
tab5
Table 5: Pseudocode and computational expense evaluation of the MGSPF algorithm.
507370.fig.001
Figure 1: Geometry simplex sigma points in 3-dimensional Euclidean space.

(i) The new set is more intuitive to comprehend and apply, especially for the 3-dimensional Euclidean space, the true space where we are, and the true space in which a variety of dynamical problems as guidance, navigation, and so on take place.

(ii) Lower computation expense and better round-off error behavior. The new set is free from calculating any irrational numbers. Furthermore, as it is only constituted of ±1, we can replace the multiplication operations in (3.12), (3.16) with simple sign changes. By elimination of both irrational number and multiplication, we made (3.12), (3.16) precise, and free from round-off errors.

(iii) The Geometric simplex set has a better symmetrical structure, which would help to further increase the numerical accuracy, including (a) single dimension symmetry completely fulfilled (or in matrix language, each row of 𝖀 is constituted with symmetrically distributed elements). (b) interdimensional symmetry (or per mutational symmetry) partly fulfilled. Define the generator point as 𝐮=[1,1,1]𝑇, and construct new points from 𝐮 by permutation and sign-changing, altogether we can make 8 points occupying the 8 vertices of the unit cube in Figure 1. Note 𝖀 has included 4 of them with a symmetric structure, which is enough to capture the random state’s first two order moments (mean and variance). Actually, the other 4 points can be found in 𝖀, and clearly, for an unbiased problem, choosing either 𝖀 or 𝖀 is equivalent.

Numerical Demonstration
Suppose that we have already obtained a 3-dimensional unbiased state 𝐚, covariance 𝐏, and its coresponding square-root matrix 𝐒. Then we are going to generate a set of sigma points with a base set 𝖀 as 𝑎=𝐒𝖀. As had been claimed, theoretically we should have 𝐚=𝑊𝑚𝜶(1)++𝜶(𝑚)=𝟎,𝐏=𝑎𝚲𝖂(𝑎)𝑇=𝐒𝐒𝑇,(3.24) where 𝐚 and 𝐏 represent the result by numerical computation. Then we can take the norm of the residues |𝐚𝟎| and |diag(𝐏𝐏)| as a criterion of a sigma set's numerical accuracy. Three typical base sets, namely, the Spherical Simplex set, the Schmidt Orthogonal set and the new proposed Geometry Simplex set are compared over a series of different 𝑆 matrices with their diagonal elements ranked form 102106.
The numerical experiment is programmed with double precision float numbers in MATLAB, and some typical results are listed in Table 2. As can be seen, numerical behaviors of the Geometric simplex set are quite encouraging. On mean computation, both spherical and Schmidt sets introduced a residue error at the scale of about 1016 of the diagonal elements of 𝐒, while the geometric set’s computed residue had always been precisely 0. On covariance computation, the new set's accuracy is also significantly superior. Biased sets are also studied and the result is similar.

3.3. The Highly Efficient New Filter

Incorporating the Geometric Simplex sigma point set into the Marginal SPKF framework, we would have a new nonlinear SPKF estimator for attitude estimation, namely, the Marginal Geometric Sigma Point Filter (MGSPF) algorithm, summarized in Table 5. As a sigma point filtering algorithm, the MGSPF has a significant increase in numerical efficiency, while it still guarantees a same order precision as the traditional SPKF algorithms. A general comparison of computation expense is taken between the MEKF and MGSPF algorithms as listed in Tables 4 and 5, and the result is summarized in Table 3.

For the computing effort of the propagation phase, there is little difference between MGSPF and MEKF, both are about half of the SSUKFs. For measurement update phase, the MGSPF takes some more arithmetic operations, but still only 80% of the SSUFK. In fact, if we take into account that in most actual implementations, there exist more propagation steps than observation steps, the total computational expense of MEKF and MGSPF would be very close. It is clear that the MGSPF has achieved a truly rivalizing efficiency as the MEKF, even when simple analytical closed-form solutions are included in the MEKF, and they are almost 50% of the SSUKF.

4. Simulations

In this section we apply the proposed Marginal Geometric Sigma Point Filter (MGSPF) algorithm to the typical stellar-inertial spacecraft attitude determination system with numerical simulations. To give a comparison, the multiple extended Kalman filter (MEKF) and a nonaugmented spherical simplex unscented Kalman filter (SSUKF) with trapezoidal approximation of the propagation noise are also simulated.

Parameters of the simulated model are set as follows. The spacecraft’s initiation attitude is 𝐪0=[0.1,0.15,0.2,1]𝑇, or expressed in 3-1-2 Euler Angles as [14°,15°,21°]. The initial angular velocity is 𝝎0=[0.05/s,0.1/s,0.15/𝑠]𝑇, and it runs a sinusoidal maneuver at an Amplitude of 0.5°/s and periods of 100 s, 120 s, and 125 s for each axis. The gyroscope is modeled as initial bias 3.4°/s, drift instability (also known as the flicker noise) 0.001°/s; angular random walk (ARW) 1×103/s1/2, rate random walk 1.4×103/s3/2, and sampling frequency 20 Hz. The star sensor is simulated with 1𝜎 accuracy as cross boresight 10 arc-seconds and around boresight 30 arc-seconds, and the sensor’s update-rate is 5 Hz.

The initial states of all filters are set equivalently as ̂𝐱0̂𝐚=[𝑇0̂𝐛,Δ𝑇0]𝑇=𝟎6×1, ̂𝐪0=[𝟎𝑇3×1,1]𝑇, and ̂𝐛0=𝟎3×1. The initial covariance 𝐏𝑋0is set with the attitude-related elements 𝐏𝐴0=(10)2𝐈3, and bias-related elements 𝐏𝐵0=(0.1/𝑠)2𝐈3. For MGSPF, as 𝐏𝐵0 is no longer used, we equivalently set a 𝐏0𝐵𝐴=𝐏𝐴0𝐏𝐵0. Specific elements in 𝐑𝑘 and 𝐐𝑘 are chosen through tuning, set as 𝜎𝑟=2×107,𝜎ARW=4.5×104, and 𝜎RRW=4×104, respectively, for all three filters.

The simulation results of attitude estimation error and and gyro bias estimation error are, respectively, illustrated in Figures 2 and 3. As the star sensor has a high precision and a 5 Hz Data Update Rate, the three filters’ steady-state accuracies are close to each other; so we mainly focus on the initial stage of the estimation process. As shown in Figure 2, of the attitude estimation error, to converge to a value below 0.001°, MEKF takes more than 60 star observations, SSUKF takes about 40, while the MGSPF takes only about 20. Meanwhile, as in Figure 3 of the gyro bias estimation, to achieve an estimation precision of 0.001°/s, MEKF takes more than 50 star samples, SSUKF takes 30, and the MGSPF takes about 20. This indicates that the MGSPF algorithm, once properly implemented, provides a better performance than MEKF at a similar numerical expense, while it is able to achieve, if not better, at least a comparable performance to traditional sigma point filters, at a significant lower expense.

507370.fig.002
Figure 2: Estimation error history of the attitude.
507370.fig.003
Figure 3: Estimation error history of the gyroscope bias.

We now address the issue of tuning. The main difference between MGSPF and traditional sigma point filters in parameter selection is mainly reflected in the usage of 𝜎RRW; so we focus on the effect of different 𝜎RRW on the performance of MGSPF. As illustrated in Figure 4, a larger 𝜎RRW has the advantage of enabling a faster convergence or can also be said as an enhancement of the filter's tracking ability. On the other hand, a large 𝜎RRW also has a drawback of instable parameter estimation in steady state; that is, the estimation of Δ𝐛 will “jump”. An optimized value of 𝜎RRW would be a tradeoff between the two. In addition, as the sigma points are created from the covariance matrices which are added with the noise terms arisen from 𝜎RRW, the scale of 𝜎RRW should be kept in a reasonable range that would not obscure the information contained in the covariances.

507370.fig.004
Figure 4: Influence of the bias noise parameter in MGSPF.

5. Conclusion

A new, minimum sigma points algorithm for spacecraft attitude and angular rate estimation has been developed. By marginalizing out the linear substructures within the random walk gyro bias model and the attitude involving, only observation model, the new algorithm needs only 4 sigma points to give a complete 6-state attitude and angular rate estimation. The algorithm’s computational expense is only 50% of the traditional SSUKF algorithm. It has truly rivaled the MEKF algorithm’s computing speed even when simple analytical closed-form solutions are included. Yet it is still able to achieve the same accuracy as traditional unscented Kalman filters.

A new, symmetrical, and numerically more efficient simplex sigma set has been presented. The new set is completely free from irrational numbers and is free from any multiplication operations during sigma point construction. The new set introduces almost none of round-off error for mean reference and smaller error for covariance reference. It would be of use for the implementation in a variety of 3-dimensional Euclidean space involving dynamical problems such as positioning and attitude estimation problems.

With the remarkable reduction in computational expense, the sigma point Kalman filter would gain a significant upgrading in its competitiveness as a candidate algorithm for actual onboard implementation.

References

  1. E. J. Lefferts, F. L. Markley, and M. D. Shuster, “Kalman filtering for spacecraft attitude estimation,” Journal of Guidance, Control, and Dynamics, vol. 5, no. 5, pp. 417–429, 1982. View at Google Scholar · View at Scopus
  2. S. J. Julier and J. K. Uhlmann, “Unscented filtering and nonlinear estimation,” Proceedings of the IEEE, vol. 92, no. 3, pp. 401–422, 2004. View at Publisher · View at Google Scholar · View at Scopus
  3. R. D. van Merwe, E. A. Wan, and S. I. Julier, “Sigma-point Kalman filters for nonlinear estimation and sensor-fusion—applications to integrated navigation,” in Proceedings of the Guidance, Navigation, and Control Conference (AIAA '04), vol. 3, pp. 1735–1764, Providence, RI, USA, August 2004.
  4. S. J. Julier, J. K. Uhlmann, and H. F. Durrant-Whyte, “New approach for filtering nonlinear systems,” in Proceedings of the American Control Conference (ACC '95), vol. 3, pp. 1628–1632, Seattle, Wash, USA, 1995.
  5. J. L. Crassidis and F. L. Markley, “Unscented filtering for spacecraft attitude estimation,” Journal of Guidance, Control, and Dynamics, vol. 26, no. 4, pp. 536–542, 2003. View at Google Scholar · View at Scopus
  6. K. Lai, J. Crassidis, and R. Harman, “In-space spacecraft alignment calibration using the unscented filter,” in Proceedings of the Guidance, Navigation, and Control Conference and Exhibit (AIAA '03), Honolulu, Hawaii, USA, August 2003.
  7. J. Côté and J. de Lafontaine, “Magnetic-only orbit and attitude estimation using the square-root unscented Kalman filter: application to the PROBA-2 spacecraft,” in Proceedings of the Guidance, Navigation, and Control Conference and Exhibit (AIAA '08), Honolulu, Hawaii, USA, August 2008.
  8. R. van der Merwe and E. Wan, “The square-root unscented Kalman filter for state and parameter-estimation,” in Proceedings of the IEEE International Conference on Acoustics, Speech and Signal Processing (ICASSP '01), vol. 6, pp. 3461–3464, Salt Lake City, Utah, USA, May 2001.
  9. S. J. Julier and J. K. Uhlmann, “Reduced sigma point filters for the propagation of means and covariances through nonlinear transformations,” in Proceedings of the American Control Conference (ACC '02), vol. 2, pp. 887–892, Anchorage, Alaska, USA, May 2002.
  10. S. J. Julier, “The spherical simplex unscented transformation,” in Proceedings of the American Control Conference (ACC '03), vol. 3, pp. 2430–2434, Denver, Colo, USA, June 2003.
  11. J. G. Castrejôn-Lozano, L. R. Garca Carrillo, A. Dzul, and R. Lozano, “Spherical simplex sigma-point Kalman filters: a comparison in the inertial navigation of a terrestrial vehicle,” in Proceedings of the American Control Conference (ACC '08), pp. 3536–3541, 2008. View at Publisher · View at Google Scholar
  12. J.-F. Lévesque, “Second-order simplex sigma points for nonlinear estimation,” in Proceedings of the Guidance, Navigation, and Control Conference (AIAA '06), vol. 2, pp. 819–830, Keystone, Colo, USA, August 2006.
  13. S. J. Julier, “The scaled unscented transformation,” in Proceedings of the American Control Conference (ACC '02), vol. 6, pp. 4555–4559, Anchorage, Alaska, USA, May 2002.
  14. W.-C. Li, P. Wei, and X.-C. Xiao, “A novel simplex unscented transform and filter,” in Proceedings of the International Symposium on Communications and Information Technologies (ISCIT '07), pp. 926–931, Sydney, Australia, 2007. View at Publisher · View at Google Scholar
  15. I. Arasaratnam and S. Haykin, “Cubature Kalman filters,” IEEE Transactions on Automatic Control, vol. 54, no. 6, pp. 1254–1269, 2009. View at Publisher · View at Google Scholar · View at MathSciNet
  16. M. R. Morelande and B. Ristic, “Smoothed state estimation for nonlinear Markovian switching systems,” IEEE Transactions on Aerospace and Electronic Systems, vol. 44, no. 4, pp. 1309–1325, 2008. View at Publisher · View at Google Scholar · View at Scopus
  17. F. L. Markley, “Attitude error representations for Kalman filtering,” Journal of Guidance, Control, and Dynamics, vol. 26, no. 2, pp. 311–317, 2003. View at Publisher · View at Google Scholar · View at Scopus
  18. P. Singla, A new attitude determination approach using split field of view star camera, M.S. thesis, Texas A&M University, College Station, Tex, USA, 2002.
  19. R. L. Farrenkopf, “Analytic steady-state accuracy solutions for two common spacecraft attitude estimators,” Jounal of Guidance and Control, vol. 1, no. 4, pp. 282–284, 1978. View at Google Scholar