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 [5–7].
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 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 —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 . 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 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 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 , with and , where is Euler axis and is the rotation angle. Quaternion parameter satisfies a single constraint given by . The kinematics equation is given by
where is the angular rate vector given from the gyro’s measurement by compensating the gyro bias :
where is a zero-mean Gaussian angular random walk noise with a covariance of . is often modeled as a rate random walk process with white noise and a covariance of :
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 to :
where
with
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 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):
with . Clearly, such MRP-based expression is quite economic for computation. Then the discrete-time propagation equations can be written as follows:
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:
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
where is the local error quaternion corresponding to . Again we choose to use the MRP approximation, as
with . 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 . 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:
where and is the measurement noise covariance modeled as
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,
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
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 , (see Table 4) [18], and [19] as Moreover, involves sparse and unit matrices as
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 :
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: For clarity, write the sigma points and their associated weight in matrix form, as
For unbiased distribution of , is constructed as
where is an arbitrary square-root matrix of with , also denoted as . 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: 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 , the local error state is reset to zero:
Then we construct the sigma points of . As in [5], we would like to use the trapezoidal approximation to avoid state augmentation; so actually is computed as
where
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:
where , and Thereby we have the propagated sigma points and , respectively, as Note we have propagated the attitude-related sigma points directly without adhering it to the global state 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 Now we compute the predicted measurements. Noting that the sigma points have already implicitly changed their reference from to during propagation, so the predicted star sensor quaternion measurements are
Following (2.12) and (2.13), it is straight forward to derive the observation model for the SPKF as Denoted in matrix form as , then we can get the predicted measurement as
Next we compute the covariance predictions. If we take 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:
where “” denotes the Kronecker product, and . Similarly, we give the unbiased measurement points set as
the innovation covariance and cross covariance matrix are then, respectively, and the measurement update procedure is
Finally, update the global states and as
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 remain unchanged during the whole process of the propagation, indicating that their mean would also remain unchanged as . 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 are explicitly used to construct the measurement predictions . We can write a formal expression of this transform as
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 .
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, , , and (b) the covariance estimation as .
To match the unbiased mean, we have
As stated in [9], to fully capture the mean of an n-dimensional state vector, at least points are needed. Noting both and , the minimum which satisfies both (3.2) and (3.3) is .
To further reduce the computational expense and make a better symmetry property, assign equal weights for all the sigma points. Then we have
Clearly,
To match ’s covariance estimation with outer products approximation, we have
Again we make use of a base set to help matching . Denoting , the following relationship should be satisfied:
Substituting (3.5) and (3.6), it is straightforward to get
As , 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 where is an arbitrary matrix square-root of fulfilling . It is straightforward to validate (3.12) as substituting it to (3.7):
To match the cross covariance is
Here we propose a simple and convenient algorithm to compute . Define , which we could get with the low computational cost Gaussian elimination:
Then we have Proof of (3.15) is straightforward as substituting it to (3.14):
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
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
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
Then similar to [11], we can add the noise terms directly to the sigma points with the help of :
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 and have a common sign. Recalling (25) as
here the negative sign in the right side of (3.23) clearly indicates opposite signs between 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 . 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 , , 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.
(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 , 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 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
where and represent the result by numerical computation. Then we can take the norm of the residues and 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 .
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 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 , or expressed in 3-1-2 Euler Angles as [14°,15°,21°]. The initial angular velocity is , 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) , rate random walk 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 , , and . The initial covariance is set with the attitude-related elements =, and bias-related elements . For MGSPF, as is no longer used, we equivalently set a . Specific elements in and are chosen through tuning, set as , and , 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.
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 ; so we focus on the effect of different on the performance of MGSPF. As illustrated in Figure 4, a larger 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 also has a drawback of instable parameter estimation in steady state; that is, the estimation of will “jump”. An optimized value of 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 , the scale of should be kept in a reasonable range that would not obscure the information contained in the covariances.
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.