# Highly Efficient Sigma Point Filter for Spacecraft Attitude and Rate Estimation

Authors: Chunshi Fan; Zheng You
Journal: Mathematical Problems in Engineering
 (2009)

---

## 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.


---

## Body

## 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 denotem as the number of sigma points required, then for an n-dimension random state vector x, standard unscented transformation needs a set of m=2n+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 m needed is no longer 2n+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 m back to 2n+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 n+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 n+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 ofm. 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 asq̅=[qT,q4]T, with q≡[q1,q2,q3]T=n̂sin(ϕ/2) and q4=cos(ϕ/2), where n̂ is Euler axis and ϕ is the rotation angle. Quaternion parameter satisfies a single constraint given by q̅Tq̅=1. The kinematics equation is given by(2.1)q¯̇=12Ω(ω)q,
where ω is the angular rate vector given from the gyro’s measurement ωmeas by compensating the gyro bias b:(2.2)ω=ωmeas-b-ηARW,
where ηARW is a zero-mean Gaussian angular random walk noise with a covariance of σARW2I3. b is often modeled as a rate random walk process with white noise ηRRW and a covariance of σRRW2I3:(2.3)ḃ=ηRRW.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 T for each filtering circle from tk-1 to tk:(2.4)q¯̂k/k-1=q¯̂k-1⊗q¯̂k-1ω,
where(2.5)q¯̂k-1ω=[ϕ̂k-1T2sin(ϕ̂k-1/2)cos(ϕ̂k-1/2),cosϕ̂k-12]T
with(2.6)ω̂k-1=ωk-1meas-b̂k-1,ϕ̂k-1=ω̂k-1T,ϕ̂k-1=|ϕ̂k-1|.
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 q̅(ϕ)=[n̂Tsin(ϕ/2),cos(ϕ/2)]T to its pertinent rotation vector ϕ=n̂ϕ 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):(2.7)ϕ(δq)=4δq(1+δq4),δq̅(ϕ)=[8ϕT,(16-ϕ2)]T(16+ϕ2)
with ϕ2=ϕTϕ. Clearly, such MRP-based expression is quite economic for computation. Then the discrete-time propagation equations can be written as follows:(2.8)q¯̂k-1ω=[8ϕ̂k-1T,(16-ϕ̂k-12)]T(16+ϕ̂k-12),b̂k/k-1=b̂k-1.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 vectora and the local gyro bias error as another 3-dimensional vector Δb. Then the actual state vector processed by the filter is the 6-dimension disturbance state:(2.9)x=[aT,ΔbT]T.
For clarity, we hence address the original states q̅̂ and b 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)q̅=q̅̂⊗δq̅(â),b=b̂+Δb,
where δq̅ is the local error quaternion corresponding to â. Again we choose to use the MRP approximation, as(2.11)a(δq)=4δq(1+δq4),δq̅(a)=[8aT,(16-a2)]T(16+a2)
with a2=aTa. 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 measurementsq̅kmeas. 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:(2.12)zkmeas=h(x̂k)=a(δq̅kmeas)+vk,
where
(2.13)δq̅kmeas=q¯̂k/k-1-1⊗q̅kmeas,
and vk is the measurement noise covariance modeled as(2.14)Rk=σr2I3.
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,(2.15)Pk/k-1X=Θk-1Pk-1XΘk-1T+Qk,(2.16)Kk=Pk/k-1XHkT(HkPk/k-1XHkT+Rk)-1,(2.17)PkX=(I6-KkHk)Pk/k-1X,
where PkX is the estimation of x’s covariance, Kk is the Kalman filter gain, Hk is the observation matrix, and Θk and Qk are, respectively, the transition matrix and the propagation noise matrix, as(2.18)Θk-1=[Φk-1Ψk-103×3I3],Qk=[QkA(QkBA)TQkBAQkB].General problem would involve numerical integration of Riccati equations to evaluate theΘk matrix in (2.15), and complex derivations to evaluate the Hk matrix in (2.16) and (2.17). Fortunately, it is also found that simple analytical solutions exist for Φk-1, Ψk-1 (see Table 4) [18], and Qk [19] as
(2.19)QkA=T(σARW2+(13)σRRW2T2)I3,QkBA=-(12)σRRW2T2I3,QkB=σRRW2TI3.
Moreover, Hk involves sparse and unit matrices as(2.20)Hk=[I3,03×3].
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 ofm sigma points are chosen to approximate the statistic distribution of the 6-dimension disturbance state x=[aT,ΔbT]T:(2.21)χ(i)=[α(i)T,β(i)T]T,i=1,2,…,m,
where (i) is the index, α(i), β(i) represent the attitude and bias error, respectively, each point is assigned with a weight W(i), and all the weights satisfy the normalization constraint:
(2.22)∑i=1mW(i)=1.
For clarity, write the sigma points and their associated weight in matrix form, as(2.23)𝒳=[χ(1),χ(2),…,χ(m)]=[𝒶ℬ]=[α(1),α(2),…,α(m)β(1),β(2),…,β(m)],𝒲=[W(1),W(2),…,W(m)],Λ𝒲=diag(𝒲).
For unbiased distribution of x∈ℝn, 𝒳 is constructed as(2.24)𝒳=SX𝒰,
where SX is an arbitrary square-root matrix of PX with SX(SX)T=PX=cov(x,x)̂, also denoted as SX=PX. 𝒰=[u(1),…,u(m)]∈ℝn×m 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)𝒰·𝒲T=0,𝒰·ΛW·𝒰T=Im.
The construction strategy of 𝒰 is also the dominating differentiation between different SPKF algorithms. With the definitions above, the set X is able to capture the statistics of x’s distribution precisely up to 2nd-order.The SPKF attitude estimator is as follows. At the beginning time of each filtering steptk-1, the local error state is reset to zero:(2.26)x̂k-1=[âk-1T,Δb̂k-1T]T=[03×1,03×1]T.
Then we construct the sigma points of Xk-1. As in [5], we would like to use the trapezoidal approximation to avoid state augmentation; so actually Sk-1X is computed as(2.27)Sk-1X=Pk-1X+Q̅k-1X,
where(2.28)Q̅k-1X=[Q̅k-1A,03×303×3,Q̅k-1B]=[T2(σARW2-T26σRRW2)I3,03×303×3,T2σRRW2I3],
and it is equivalent to have the Qk in (2.18) implicitly propagated together with the sigma points [5].Next, propagate them sigma points as follows:(2.29)ϕk-1β(i)=(ω̂k-1-βk-1(i))T,q̅k-1β(i)=[8ϕk-1β(i)T,(16-ϕk-1β(i)2)]T(16+ϕk-1β(i)2),
where ϕk-1β(i)=|ϕk-1β(i)|, and
(2.30)q̅k-1α(i)=[8αk-1(i)T,(16-αk-1(i)2)]T(16+αk-1(i)2),q̅k/k-1α(i)=(q¯̂k-1ω)-1⊗q̅k-1α(i)⊗q̅k-1β(i).
Thereby we have the propagated sigma points 𝒶k/k-1 and ℬk/k-1, respectively, as
(2.31)αk/k-1(i)=α(q̅k/k-1α(i))=4qk/k-1α(i)(1+q4k/k-1α(i)),(2.32)βk/k-1(i)=βk-1(i).
Note we have propagated the attitude-related sigma points α(i) directly without adhering it to the global state q¯̂k-1 as in [5]; so a decrease of computational complexity is achievable. This approach stands as far as the time interval of T is guaranteed to be small enough in order that both α(i) and ϕ(i) can be taken as small rotation vectors, and the MRP approximation is valid. Then the predicted mean of x is
(2.33)x̂k/k-1=𝒳k/k-1𝒲T.
Now we compute the predicted measurements. Noting that the sigma points have already implicitly changed their reference from q¯̂k-1 to q¯̂k/k-1 during propagation, so the predicted star sensor quaternion measurements are(2.34)q¯̂k/k-1χ(i)=q¯̂k/k-1⊗δq¯k/k-1α(i).
Following (2.12) and (2.13), it is straight forward to derive the observation model for the SPKF as
(2.35)ζk/k-1(i)=αk/k-1(i).
Denoted in matrix form as 𝒵=[ζ(1),ζ(2),…,ζ(m)], then we can get the predicted measurement 𝓏̂k/k-1 as(2.36)𝓏̂k/k-1=Zk/k-1·WT.Next we compute the covariance predictions. If we takex̂k/k-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:(2.37)𝒳k/k-1=𝒳k/k-1-11×m∘x̂k/k-1,(2.38)Pk/k-1X=Xk/k-1·ΛW·Xk/k-1T,
where “∘” denotes the Kronecker product, and 11×m=[1,1,…,1]1×m. Similarly, we give the unbiased measurement points set as(2.39)Zk/k-1=Zk/k-1-11×m∘𝓏̂k/k-1
the innovation covariance and cross covariance matrix are then, respectively,
(2.40)Pk/k-1Z=Zk/k-1·Λ𝒲·Zk/k-1T+Rk,Pk/k-1XZ=Xk/k-1·Λ𝒲·Zk/k-1T
and the measurement update procedure is(2.41)Kk=Pk/k-1XZ(Pk/k-1Z)-1,(2.42)x̂k=x̂k/k-1+Kk(𝓏̂kmeas-𝓏̂k/k-1),(2.43)PkX=Pk/k-1X-KkPk/k-1Z(Kk)T.
Finally, update the global states q̅̂ and b̂ as(2.44)q¯̂k=q¯̂k/k-1⊗δq¯(âk),b̂k=b̂k/k-1+Δb̂k.Above is the framework of an SPKF version attitude estimator. A blocked-form procedure summary is listed in Table5. 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.


## 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 asq̅=[qT,q4]T, with q≡[q1,q2,q3]T=n̂sin(ϕ/2) and q4=cos(ϕ/2), where n̂ is Euler axis and ϕ is the rotation angle. Quaternion parameter satisfies a single constraint given by q̅Tq̅=1. The kinematics equation is given by(2.1)q¯̇=12Ω(ω)q,
where ω is the angular rate vector given from the gyro’s measurement ωmeas by compensating the gyro bias b:(2.2)ω=ωmeas-b-ηARW,
where ηARW is a zero-mean Gaussian angular random walk noise with a covariance of σARW2I3. b is often modeled as a rate random walk process with white noise ηRRW and a covariance of σRRW2I3:(2.3)ḃ=ηRRW.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 T for each filtering circle from tk-1 to tk:(2.4)q¯̂k/k-1=q¯̂k-1⊗q¯̂k-1ω,
where(2.5)q¯̂k-1ω=[ϕ̂k-1T2sin(ϕ̂k-1/2)cos(ϕ̂k-1/2),cosϕ̂k-12]T
with(2.6)ω̂k-1=ωk-1meas-b̂k-1,ϕ̂k-1=ω̂k-1T,ϕ̂k-1=|ϕ̂k-1|.
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 q̅(ϕ)=[n̂Tsin(ϕ/2),cos(ϕ/2)]T to its pertinent rotation vector ϕ=n̂ϕ 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):(2.7)ϕ(δq)=4δq(1+δq4),δq̅(ϕ)=[8ϕT,(16-ϕ2)]T(16+ϕ2)
with ϕ2=ϕTϕ. Clearly, such MRP-based expression is quite economic for computation. Then the discrete-time propagation equations can be written as follows:(2.8)q¯̂k-1ω=[8ϕ̂k-1T,(16-ϕ̂k-12)]T(16+ϕ̂k-12),b̂k/k-1=b̂k-1.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 vectora and the local gyro bias error as another 3-dimensional vector Δb. Then the actual state vector processed by the filter is the 6-dimension disturbance state:(2.9)x=[aT,ΔbT]T.
For clarity, we hence address the original states q̅̂ and b 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)q̅=q̅̂⊗δq̅(â),b=b̂+Δb,
where δq̅ is the local error quaternion corresponding to â. Again we choose to use the MRP approximation, as(2.11)a(δq)=4δq(1+δq4),δq̅(a)=[8aT,(16-a2)]T(16+a2)
with a2=aTa. 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 measurementsq̅kmeas. 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:(2.12)zkmeas=h(x̂k)=a(δq̅kmeas)+vk,
where
(2.13)δq̅kmeas=q¯̂k/k-1-1⊗q̅kmeas,
and vk is the measurement noise covariance modeled as(2.14)Rk=σr2I3.
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,(2.15)Pk/k-1X=Θk-1Pk-1XΘk-1T+Qk,(2.16)Kk=Pk/k-1XHkT(HkPk/k-1XHkT+Rk)-1,(2.17)PkX=(I6-KkHk)Pk/k-1X,
where PkX is the estimation of x’s covariance, Kk is the Kalman filter gain, Hk is the observation matrix, and Θk and Qk are, respectively, the transition matrix and the propagation noise matrix, as(2.18)Θk-1=[Φk-1Ψk-103×3I3],Qk=[QkA(QkBA)TQkBAQkB].General problem would involve numerical integration of Riccati equations to evaluate theΘk matrix in (2.15), and complex derivations to evaluate the Hk matrix in (2.16) and (2.17). Fortunately, it is also found that simple analytical solutions exist for Φk-1, Ψk-1 (see Table 4) [18], and Qk [19] as
(2.19)QkA=T(σARW2+(13)σRRW2T2)I3,QkBA=-(12)σRRW2T2I3,QkB=σRRW2TI3.
Moreover, Hk involves sparse and unit matrices as(2.20)Hk=[I3,03×3].
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 ofm sigma points are chosen to approximate the statistic distribution of the 6-dimension disturbance state x=[aT,ΔbT]T:(2.21)χ(i)=[α(i)T,β(i)T]T,i=1,2,…,m,
where (i) is the index, α(i), β(i) represent the attitude and bias error, respectively, each point is assigned with a weight W(i), and all the weights satisfy the normalization constraint:
(2.22)∑i=1mW(i)=1.
For clarity, write the sigma points and their associated weight in matrix form, as(2.23)𝒳=[χ(1),χ(2),…,χ(m)]=[𝒶ℬ]=[α(1),α(2),…,α(m)β(1),β(2),…,β(m)],𝒲=[W(1),W(2),…,W(m)],Λ𝒲=diag(𝒲).
For unbiased distribution of x∈ℝn, 𝒳 is constructed as(2.24)𝒳=SX𝒰,
where SX is an arbitrary square-root matrix of PX with SX(SX)T=PX=cov(x,x)̂, also denoted as SX=PX. 𝒰=[u(1),…,u(m)]∈ℝn×m 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)𝒰·𝒲T=0,𝒰·ΛW·𝒰T=Im.
The construction strategy of 𝒰 is also the dominating differentiation between different SPKF algorithms. With the definitions above, the set X is able to capture the statistics of x’s distribution precisely up to 2nd-order.The SPKF attitude estimator is as follows. At the beginning time of each filtering steptk-1, the local error state is reset to zero:(2.26)x̂k-1=[âk-1T,Δb̂k-1T]T=[03×1,03×1]T.
Then we construct the sigma points of Xk-1. As in [5], we would like to use the trapezoidal approximation to avoid state augmentation; so actually Sk-1X is computed as(2.27)Sk-1X=Pk-1X+Q̅k-1X,
where(2.28)Q̅k-1X=[Q̅k-1A,03×303×3,Q̅k-1B]=[T2(σARW2-T26σRRW2)I3,03×303×3,T2σRRW2I3],
and it is equivalent to have the Qk in (2.18) implicitly propagated together with the sigma points [5].Next, propagate them sigma points as follows:(2.29)ϕk-1β(i)=(ω̂k-1-βk-1(i))T,q̅k-1β(i)=[8ϕk-1β(i)T,(16-ϕk-1β(i)2)]T(16+ϕk-1β(i)2),
where ϕk-1β(i)=|ϕk-1β(i)|, and
(2.30)q̅k-1α(i)=[8αk-1(i)T,(16-αk-1(i)2)]T(16+αk-1(i)2),q̅k/k-1α(i)=(q¯̂k-1ω)-1⊗q̅k-1α(i)⊗q̅k-1β(i).
Thereby we have the propagated sigma points 𝒶k/k-1 and ℬk/k-1, respectively, as
(2.31)αk/k-1(i)=α(q̅k/k-1α(i))=4qk/k-1α(i)(1+q4k/k-1α(i)),(2.32)βk/k-1(i)=βk-1(i).
Note we have propagated the attitude-related sigma points α(i) directly without adhering it to the global state q¯̂k-1 as in [5]; so a decrease of computational complexity is achievable. This approach stands as far as the time interval of T is guaranteed to be small enough in order that both α(i) and ϕ(i) can be taken as small rotation vectors, and the MRP approximation is valid. Then the predicted mean of x is
(2.33)x̂k/k-1=𝒳k/k-1𝒲T.
Now we compute the predicted measurements. Noting that the sigma points have already implicitly changed their reference from q¯̂k-1 to q¯̂k/k-1 during propagation, so the predicted star sensor quaternion measurements are(2.34)q¯̂k/k-1χ(i)=q¯̂k/k-1⊗δq¯k/k-1α(i).
Following (2.12) and (2.13), it is straight forward to derive the observation model for the SPKF as
(2.35)ζk/k-1(i)=αk/k-1(i).
Denoted in matrix form as 𝒵=[ζ(1),ζ(2),…,ζ(m)], then we can get the predicted measurement 𝓏̂k/k-1 as(2.36)𝓏̂k/k-1=Zk/k-1·WT.Next we compute the covariance predictions. If we takex̂k/k-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:(2.37)𝒳k/k-1=𝒳k/k-1-11×m∘x̂k/k-1,(2.38)Pk/k-1X=Xk/k-1·ΛW·Xk/k-1T,
where “∘” denotes the Kronecker product, and 11×m=[1,1,…,1]1×m. Similarly, we give the unbiased measurement points set as(2.39)Zk/k-1=Zk/k-1-11×m∘𝓏̂k/k-1
the innovation covariance and cross covariance matrix are then, respectively,
(2.40)Pk/k-1Z=Zk/k-1·Λ𝒲·Zk/k-1T+Rk,Pk/k-1XZ=Xk/k-1·Λ𝒲·Zk/k-1T
and the measurement update procedure is(2.41)Kk=Pk/k-1XZ(Pk/k-1Z)-1,(2.42)x̂k=x̂k/k-1+Kk(𝓏̂kmeas-𝓏̂k/k-1),(2.43)PkX=Pk/k-1X-KkPk/k-1Z(Kk)T.
Finally, update the global states q̅̂ and b̂ as(2.44)q¯̂k=q¯̂k/k-1⊗δq¯(âk),b̂k=b̂k/k-1+Δb̂k.Above is the framework of an SPKF version attitude estimator. A blocked-form procedure summary is listed in Table5. 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),…,β(m)] remain unchanged during the whole process of the propagation, indicating that their mean would also remain unchanged as Δb̂k/k-1=Δb̂k-1=03×1. In other words, no information has been introduced into the state Δb’s mean and covariance cov(Δb,Δb) during the propagation. Therefore, once we are able to capture the information of â, cov(a,a) and cov(b,a), we have already obtained all the information available during time propagation.Then noting (2.35), we find only the attitude-related sigma points 𝒶k/k-1 are explicitly used to construct the measurement predictions 𝒵k/k-1. We can write a formal expression of this transform as(3.1)𝓏=h(x)=γ(a).
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 𝓏̅, cov(𝓏,𝓏), and cross covariance cov(a,𝓏) are all independent of Δb, and it is proved that the cross covariance cov(Δb,𝓏) is also independent of cov(Δb,Δb) 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 ofâ, Δb̂, and the covariances of PA=cov(a,a)̂ and PBA=cov(Δb,a)̂, it is enough to capture the first two moments’ statistics properties of the random state x 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 x=[aT,ΔbT]T=[03×1T,03×1T]T.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,a̅=03×1, Δb̅=03×1, and (b) the covariance estimation as PA=cov(a,a)PBA=cov(Δb,a).To match the unbiased mean, we have(3.2)𝒶·WT=Wm(α(1)+α(2)+⋯+α(m))=0,(3.3)ℬ·WT=Wm(β(1)+β(2)+⋯+β(m))=0.
As stated in [9], to fully capture the mean of an n-dimensional state vector, at least m=n+1 points are needed. Noting both a and Δb∈ℝ3, the minimum m which satisfies both (3.2) and (3.3) is m=4.To further reduce the computational expense and make a better symmetry property, assign equal weightsW for all the sigma points. Then we have(3.4)∑i=1mWi=mW=1.
Clearly,
(3.5)W=1m=14,(3.6)W=(1m)11×m,ΛW=(1m)Im.To matcha’s covariance estimation PA=cov(a,a)̂ with outer products approximation, we have(3.7)𝒶ΛW(𝒶)T=(1m)𝒶(𝒶)T=PA.
Again we make use of a base set U𝒶∈ℝ3×m to help matching PA. Denoting U𝒶=[ua(1),ua(2),ua(3),ua(4)], the following relationship should be satisfied:(3.8)U𝒶·WT=0,(3.9)U𝒶·ΛW·(U𝒶)T=I3.
Substituting (3.5) and (3.6), it is straightforward to get(3.10)∑i=1mua(i)=0,(3.11)U𝒶·(U𝒶)T=mI3.
As m=4, U𝒶 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 U𝒶. Later we will propose a novel set derived from a heuristic geometry approach. Now supposing that U𝒶 is given, then we may construct 𝒶 simply as
(3.12)𝒶=SA·U𝒶,
where SA is an arbitrary matrix square-root of PA fulfilling SA(SA)T=PA. It is straightforward to validate (3.12) as substituting it to (3.7):(3.13)𝒶ΛW(𝒶)T=(1m)SA·U𝒶(SAU𝒶)T=(1m)SA·(U𝒶(U𝒶)T)·(SA)T=(1m)SA·(mI3)·(SA)T=PA.
To match the cross covariance PBA is(3.14)BΛW(𝒶)T=(1m)B(𝒶)T=PBA.
Here we propose a simple and convenient algorithm to compute B. Define Sk-1BA=PBA(SA)-T, which we could get with the low computational cost Gaussian elimination:(3.15)Sk-1BA=PBA(SA)-T=Pk-1BA(Sk-1A)T.
Then we have
(3.16)B=Sk-1BAU𝒶.
Proof of (3.15) is straightforward as substituting it to (3.14):(3.17)BΛ𝒲(𝒶)T=(1m)(Sk-1BAU𝒶)(SAU𝒶)T=(1m)Sk-1BA·(U𝒶(U𝒶)T)·(SA)T=(1m)PBA(SA)-T·(mI3)·(SA)T=PBA(SA)-T·(SA)T=PBA.
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 U𝒶 to construct the full-length Sigma point set 𝒳=[𝒶T,BT]T. So hereafter we would suppress the superscript of U𝒶 as U.Looking into (2.38), we find(3.18)PX=[PA,PABPBA,PB]=[PA,(PBA)TPBA,PB].
With the proposed sigma points, the propagation and innovation steps of Δb’s covariance estimation PB are no longer necessary. Eliminating the PB-related term and making use of the symmetric structure of the matrix, it is enough to have PA and PBA propagated. Accordingly, it is only necessary to have the same matrices updated. There by, we will replace (2.38) and (2.43) with(3.19)Pk/k-1A=𝒶k/k-1·ΛW·(𝒶k/k-1)T,Pk/k-1BA=Bk/k-1·ΛW·(𝒶k/k-1)T,PkA=Pk/k-1A-Pk/k-1AZ(Pk/k-1Z)-1(Pk/k-1AZ)T=Pk/k-1AA-KkAZ(Pk/k-1AZ)T,PkBA=Pk/k-1BA-Pk/k-1BZ(Pk/k-1Z)-1(Pk/k-1AZ)T=Pk/k-1BA-KkBZ(Pk/k-1AZ)T.To avoid state augmentation, we would like to have the propagation noise terms incorporated into the filter with trapezoidal approximation. However, asPB is no longer used, we have to seek for alternate approach. Denote(3.20)S̅k-1QA=Q̅k-1A=(T22)·(σARW2-(T26)σRRW2)·I3,S̅k-1QB=Q̅k-1B=(σRRWT2)·I3.
Then similar to [11], we can add the noise terms directly to the sigma points with the help of U:(3.21)𝒶k-1=Sk-1AU+Sk-1QAU=(Sk-1A+Sk-1QA)U,(3.22)Bk-1=Sk-1BAU-SQBU=(Sk-1BA-SQB)U.
Note in (3.22) that the sign before SQB is negative. The reason is that with a given covariance estimation P and its square-root S, it is equivalent for us to choose either S or –S 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 Sk-1BA and SQB have a common sign. Recalling (25) as(3.23)Qk-1BA=-(12)σRRW2T2I3,
here the negative sign in the right side of (3.23) clearly indicates opposite signs between Sk-1BA and SQB.


### 3.2. The Geometric Simplex Sigma Points

Construction of the base setU 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 n. 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 n=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 n.However, both sets lack numerical accuracy, and they are complex to compute, as irrational numbers2, 6, and so on exist. Further, they do not have symmetric structures. A fully symmetric set needs that for every point u(i)∈U, we can get another point u(j)∈U,i≠j simply by element permutation or sign-changing point of the generator point u(i) [15]. Clearly, except for the first dimension (or describing in matrix language, the first row of U), 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 Table1 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.Table 1
Simplex base sets for 3-dimensional space.
Sigma setU∈ℝn×m for n=3U=(0.5/Wm)[0,-1/2,1/2,0,00,-1/6,-1/6,2/6,00,-1/12,-1/12,-1/12,3/12]Spherical simplexm=50<W0<1,Wm=(1-W0)/(n+1)U=(1/W)[1/2,-1/2,0,01/6,1/6,-1/3/2,01/12,1/12,1/12,-1/4/3]Schmidt orthogonalm=4W=1/(n+1)=1/4U=[+1,+1-1-1+1,-1-1+1+1,-1+1-1]Geometric simplexm=4W=1/(n+1)=1/4Table 2
Residues of numerical mean estimation.
S[10-1,0,0;10-2,10-1,0;10-3,10-4,10-1][10-2,0,0;10-3,10-2,0;10-4,10-5,10-2][10-4,0,0;10-5,10-4,0;10-6,10-7,10-4]MeanSS2.151464×10-181.319254×10-195.591741×10-22SO1.734723×10-186.505213×10-191.694066×10-21GS0 (precise)0 (precise)0 (precise)CovarianceSS3.878960×10-184.065758×10-198.271806×10-24SO4.249187×10-183.030437×10-197.018860×10-24GS1.734723×10-180 (precise)2.339620×10-24SS: Spherical simplex set. SO: Schmidt orthogonal set. GS: Geometric simplex set.Table 3
Comparisons of total arithmetic operations.
AlgorithmPhaseMultipliesAddsSquarerootsMEKFPropagation3502702Measurement update2601851One full filter step*6104553One observation circle†168012659MGSPFPropagation3502855Measurement update3802801One full filter step7505656One observation circle1860148021SSUKFPropagation8105658Measurement update4553701One full filter step12659359One observation circle3695263033*Including 1 propagation step and 1 measurement update step.†Including several propagation steps and 1 measurement update step. In this case we take a typical value as 4 propagations for each 1 update.Table 4
Pseudocode and computational expense evaluation of the MEKF with closed-form solutions.
Algorithm×+Initializex̂0=[â0T,Δb̂0T]T=06×1, q¯̂0=[03×1T,1]T, b̂0=03×1P0A=εa2I3, P0B=εb2I3,QkA=T(σARW2+(1/3)σRRW2T2)I3QkBA=-(1/2)σRRW2T2I3,  QkB=σRRW2TI3Propagationω̂k-1=ωk-1meas-b̂k-1, ω̂k-1=|ω̂k-1|, ϕ̂k-1=ω̂k-1T651q¯̂k-1ω=[8ϕ̂k-1T,(16-ϕ̂k-12)]T/(16+ϕ̂k-12)62n̂=ω̂k-1/ω̂k-13Φk-1=I3-sinϕ̂k-1[n̂×]+(1-cosϕ̂k-1)[n̂×]2≈I3-(ϕ̂k-1-(ϕ̂k-13/6))[n̂×]+(ϕ̂k-12/2)[n̂×]24930Ψk-1=(T/2)I3-((1-cosϕ̂k-1)/ω̂k-1)[n̂k-1×]+((ϕ̂k-1-sinϕ̂k-1)/ω̂k-1)[n̂k-1×]2≈(T/2)I3-(ϕ̂k-12/2ω̂k-1)[n̂k-1×]+(ϕ̂k-13/6ω̂k-1)[n̂k-1×]25130Pk/k-1A=Φk-1Pk-1AΦk-1T+Ψk-1Pk-1BΨk-1T10881+Ψk-1Pk-1BAΦk-1T+(Ψk-1Pk-1BAΦk-1T)T+QkA5457Pk/k-1BA=Pk-1BAΦk-1T+Pk-1BΨk-1T+QkBA5448Pk/k-1B=Pk-1B+QkB3q¯̂k/k-1=q¯̂k-1⊗q¯̂k-1ω, q¯̂k/k-1=q¯̂k/k-1/∥q¯̂k/k-1∥22151b̂k/k-1=b̂k-13532712Measurement updatePk/k-1Z=HkPk/k-1XHkT+Rk=Pk/k-1A+Rk3Pk/k-1AZ=Pk/k-1AHkT=Pk/k-1APk/k-1BZ=Pk/k-1BAHkT=Pk/k-1BAKkAZ=Pk/k-1A(Pk/k-1Z)-1, KkBZ=Pk/k-1BA(Pk/k-1Z)-111458δq̅kmeas=q¯̂k/k-1-1⊗q̅kmeas1612𝓏kmeas=4qkmeas/(1+q4kmeas), γkmeas=𝓏̂kmeas54âk=KkAZγkmeas, Δb̂k=KkBZγkmeas189PkA=Pk/k-1A-KkAZ(Pk/k-1AZ)T2727PkBA=Pk/k-1BA-KkBZ(Pk/k-1AZ)T2727PkB=Pk/k-1B-Kk/k-1BZ(Pk/k-1BZ)T2727q¯̂k=q¯̂k/k-1⊗δq¯̂(âk),  q¯̂k=q¯̂k/∥q¯̂k∥22151b̂k=b̂k/k-1+Δb̂k32561851Table 5
Pseudocode and computational expense evaluation of the MGSPF algorithm.
Algorithm×+Initializex̂0=[â0T,Δb̂0T]T=06×1, q¯̂0=[03×1T,1]T,  b̂0=03×1P0A=εa2I3, P0BA=εaεbI3    m= 4S̅k-1QA=T((6σARW2-T2σRRW2)/12)I3S̅k-1QB=(σRRWT/2)I3PropagationSk-1A=Pk-1A, Sk-1BA=Pk-1BA/(Sk-1A)T25133𝒶k-1=(Sk-1A+Sk-1QA)UGS, Bk-1=(Sk-1BA-Sk-1QB)UGS054ω̂k-1=ωk-1meas-b̂k-1, ω̂k-1=|ω̂k-1|, ϕ̂k-1=ω̂k-1T651q¯̂k-1ω=[8ϕ̂k-1T,(16-ϕ̂k-12)]T/(16+ϕ̂k-12)62Fori=1:m,  ϕk-1β(i)=(ω̂k-1-βk-1(i))T3m3mq¯k-1β(i)=[8ϕk-1β(i)T,(16-ϕk-1β(i)2)]T/(16+ϕk-1β(i)2)6m2mq¯k-1α(i)=[8αk-1(i)T,(16-αk-1(i)2)]T/(16+αk-1(i)2)6m2mq¯k/k-1α(i)=(q¯̂k-1ω)-1⊗q¯k-1α(i)⊗q¯k-1β(i)32m24mαk/k-1(i)=4qk/k-1α(i)/(1+q4k/k-1α(i)) end5m1mBk/k-1=Bk-1, Zk/k-1=𝒶k/k-1âk/k-1=(1/m)𝒶k/k-1·11×m1×m, Δb̂k/k-1=Δb̂k-1=03×133(m-1)𝒶k/k-1=𝒶k/k-1-11×m∘âk/k-13mPk/k-1A=(1/m)𝒶k/k-1(𝒶k/k-1)T6+6m6(m-1)Pk/k-1BA=(1/m)Bk/k-1(𝒶k/k-1)T9+9m9(m-1)q¯̂k/k-1=q¯̂k-1⊗q¯̂k-1ω, q¯̂k/k-1=q¯̂k/k-1/∥q¯̂k/k-1∥22151b̂k/k-1=b̂k-1Form=43512855Measurement update𝓏̂k/k-1=(1/m)Zk/k-1•1m×133(m-1)Zk/k-1=Zk/k-1-11×m∘𝓏̂k/k-13mPk/k-1Z=(1/m)Zk/k-1Zk/k-1T+Rk6+6m9(m-1)+3Pk/k-1AZ=(1/m)ak/k-1Zk/k-1T9+9m9(m-1)Pk/k-1BZ=(1/m)Bk/k-1Zk/k-1T9+9m9(m-1)KkAZ=Pk/k-1A(Pk/k-1Z)-1, KkBZ=Pk/k-1BA(Pk/k-1Z)-111458δq¯kmeas=q¯k/k-1-1⊗q¯kmeas1612𝓏kmeas=4qkmeas/(1+q4kmeas), γkmeas=𝓏̂kmeas-𝓏̂k/k-154âk=KkAZγkmeas, Δb̂k=KkBZγkmeas1812PkA=Pk/k-1A-KkAZ(Pk/k-1AZ)T2727PkBA=Pk/k-1BA-KkBZ(Pk/k-1AZ)T2727PkB=Pk/k-1B-Kk/k-1BZ(Pk/k-1BZ)T2727q¯̂k=q¯̂k/k-1⊗δq¯(âk), q¯̂k=q¯̂k/∥q¯̂k∥22151b̂k=b̂k/k-1+Δb̂k3Form=43792781Figure 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 ofU is constituted with symmetrically distributed elements). (b) interdimensional symmetry (or per mutational symmetry) partly fulfilled. Define the generator point as u=[1,1,-1]T, and construct new points from u by permutation and sign-changing, altogether we can make 8 points occupying the 8 vertices of the unit cube in Figure 1. Note U 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 -U, and clearly, for an unbiased problem, choosing either U or -U is equivalent.Numerical Demonstration
Suppose that we have already obtained a 3-dimensional unbiased statea, covariance P, and its coresponding square-root matrix S. Then we are going to generate a set of sigma points with a base set U  as 𝒶=S·U. As had been claimed, theoretically we should have
(3.24)a⌢=Wm(α(1)+⋯+α(m))=0,P⌢=𝒶·ΛW·(𝒶)T=S·ST,
where a⌢ and P⌢  represent the result by numerical computation. Then we can take the norm of the residues |a⌢-0| and |diag(P⌢-P)| 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 S matrices with their diagonal elements ranked form 102~10-6.
The numerical experiment is programmed with double precision float numbers in MATLAB, and some typical results are listed in Table2. 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 10-16 of the diagonal elements of S, 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 Table5. 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.


## 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),…,β(m)] remain unchanged during the whole process of the propagation, indicating that their mean would also remain unchanged as Δb̂k/k-1=Δb̂k-1=03×1. In other words, no information has been introduced into the state Δb’s mean and covariance cov(Δb,Δb) during the propagation. Therefore, once we are able to capture the information of â, cov(a,a) and cov(b,a), we have already obtained all the information available during time propagation.Then noting (2.35), we find only the attitude-related sigma points 𝒶k/k-1 are explicitly used to construct the measurement predictions 𝒵k/k-1. We can write a formal expression of this transform as(3.1)𝓏=h(x)=γ(a).
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 𝓏̅, cov(𝓏,𝓏), and cross covariance cov(a,𝓏) are all independent of Δb, and it is proved that the cross covariance cov(Δb,𝓏) is also independent of cov(Δb,Δb) 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 ofâ, Δb̂, and the covariances of PA=cov(a,a)̂ and PBA=cov(Δb,a)̂, it is enough to capture the first two moments’ statistics properties of the random state x 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 x=[aT,ΔbT]T=[03×1T,03×1T]T.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,a̅=03×1, Δb̅=03×1, and (b) the covariance estimation as PA=cov(a,a)PBA=cov(Δb,a).To match the unbiased mean, we have(3.2)𝒶·WT=Wm(α(1)+α(2)+⋯+α(m))=0,(3.3)ℬ·WT=Wm(β(1)+β(2)+⋯+β(m))=0.
As stated in [9], to fully capture the mean of an n-dimensional state vector, at least m=n+1 points are needed. Noting both a and Δb∈ℝ3, the minimum m which satisfies both (3.2) and (3.3) is m=4.To further reduce the computational expense and make a better symmetry property, assign equal weightsW for all the sigma points. Then we have(3.4)∑i=1mWi=mW=1.
Clearly,
(3.5)W=1m=14,(3.6)W=(1m)11×m,ΛW=(1m)Im.To matcha’s covariance estimation PA=cov(a,a)̂ with outer products approximation, we have(3.7)𝒶ΛW(𝒶)T=(1m)𝒶(𝒶)T=PA.
Again we make use of a base set U𝒶∈ℝ3×m to help matching PA. Denoting U𝒶=[ua(1),ua(2),ua(3),ua(4)], the following relationship should be satisfied:(3.8)U𝒶·WT=0,(3.9)U𝒶·ΛW·(U𝒶)T=I3.
Substituting (3.5) and (3.6), it is straightforward to get(3.10)∑i=1mua(i)=0,(3.11)U𝒶·(U𝒶)T=mI3.
As m=4, U𝒶 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 U𝒶. Later we will propose a novel set derived from a heuristic geometry approach. Now supposing that U𝒶 is given, then we may construct 𝒶 simply as
(3.12)𝒶=SA·U𝒶,
where SA is an arbitrary matrix square-root of PA fulfilling SA(SA)T=PA. It is straightforward to validate (3.12) as substituting it to (3.7):(3.13)𝒶ΛW(𝒶)T=(1m)SA·U𝒶(SAU𝒶)T=(1m)SA·(U𝒶(U𝒶)T)·(SA)T=(1m)SA·(mI3)·(SA)T=PA.
To match the cross covariance PBA is(3.14)BΛW(𝒶)T=(1m)B(𝒶)T=PBA.
Here we propose a simple and convenient algorithm to compute B. Define Sk-1BA=PBA(SA)-T, which we could get with the low computational cost Gaussian elimination:(3.15)Sk-1BA=PBA(SA)-T=Pk-1BA(Sk-1A)T.
Then we have
(3.16)B=Sk-1BAU𝒶.
Proof of (3.15) is straightforward as substituting it to (3.14):(3.17)BΛ𝒲(𝒶)T=(1m)(Sk-1BAU𝒶)(SAU𝒶)T=(1m)Sk-1BA·(U𝒶(U𝒶)T)·(SA)T=(1m)PBA(SA)-T·(mI3)·(SA)T=PBA(SA)-T·(SA)T=PBA.
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 U𝒶 to construct the full-length Sigma point set 𝒳=[𝒶T,BT]T. So hereafter we would suppress the superscript of U𝒶 as U.Looking into (2.38), we find(3.18)PX=[PA,PABPBA,PB]=[PA,(PBA)TPBA,PB].
With the proposed sigma points, the propagation and innovation steps of Δb’s covariance estimation PB are no longer necessary. Eliminating the PB-related term and making use of the symmetric structure of the matrix, it is enough to have PA and PBA propagated. Accordingly, it is only necessary to have the same matrices updated. There by, we will replace (2.38) and (2.43) with(3.19)Pk/k-1A=𝒶k/k-1·ΛW·(𝒶k/k-1)T,Pk/k-1BA=Bk/k-1·ΛW·(𝒶k/k-1)T,PkA=Pk/k-1A-Pk/k-1AZ(Pk/k-1Z)-1(Pk/k-1AZ)T=Pk/k-1AA-KkAZ(Pk/k-1AZ)T,PkBA=Pk/k-1BA-Pk/k-1BZ(Pk/k-1Z)-1(Pk/k-1AZ)T=Pk/k-1BA-KkBZ(Pk/k-1AZ)T.To avoid state augmentation, we would like to have the propagation noise terms incorporated into the filter with trapezoidal approximation. However, asPB is no longer used, we have to seek for alternate approach. Denote(3.20)S̅k-1QA=Q̅k-1A=(T22)·(σARW2-(T26)σRRW2)·I3,S̅k-1QB=Q̅k-1B=(σRRWT2)·I3.
Then similar to [11], we can add the noise terms directly to the sigma points with the help of U:(3.21)𝒶k-1=Sk-1AU+Sk-1QAU=(Sk-1A+Sk-1QA)U,(3.22)Bk-1=Sk-1BAU-SQBU=(Sk-1BA-SQB)U.
Note in (3.22) that the sign before SQB is negative. The reason is that with a given covariance estimation P and its square-root S, it is equivalent for us to choose either S or –S 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 Sk-1BA and SQB have a common sign. Recalling (25) as(3.23)Qk-1BA=-(12)σRRW2T2I3,
here the negative sign in the right side of (3.23) clearly indicates opposite signs between Sk-1BA and SQB.


## 3.2. The Geometric Simplex Sigma Points

Construction of the base setU 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 n. 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 n=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 n.However, both sets lack numerical accuracy, and they are complex to compute, as irrational numbers2, 6, and so on exist. Further, they do not have symmetric structures. A fully symmetric set needs that for every point u(i)∈U, we can get another point u(j)∈U,i≠j simply by element permutation or sign-changing point of the generator point u(i) [15]. Clearly, except for the first dimension (or describing in matrix language, the first row of U), 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 Table1 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.Table 1
Simplex base sets for 3-dimensional space.
Sigma setU∈ℝn×m for n=3U=(0.5/Wm)[0,-1/2,1/2,0,00,-1/6,-1/6,2/6,00,-1/12,-1/12,-1/12,3/12]Spherical simplexm=50<W0<1,Wm=(1-W0)/(n+1)U=(1/W)[1/2,-1/2,0,01/6,1/6,-1/3/2,01/12,1/12,1/12,-1/4/3]Schmidt orthogonalm=4W=1/(n+1)=1/4U=[+1,+1-1-1+1,-1-1+1+1,-1+1-1]Geometric simplexm=4W=1/(n+1)=1/4Table 2
Residues of numerical mean estimation.
S[10-1,0,0;10-2,10-1,0;10-3,10-4,10-1][10-2,0,0;10-3,10-2,0;10-4,10-5,10-2][10-4,0,0;10-5,10-4,0;10-6,10-7,10-4]MeanSS2.151464×10-181.319254×10-195.591741×10-22SO1.734723×10-186.505213×10-191.694066×10-21GS0 (precise)0 (precise)0 (precise)CovarianceSS3.878960×10-184.065758×10-198.271806×10-24SO4.249187×10-183.030437×10-197.018860×10-24GS1.734723×10-180 (precise)2.339620×10-24SS: Spherical simplex set. SO: Schmidt orthogonal set. GS: Geometric simplex set.Table 3
Comparisons of total arithmetic operations.
AlgorithmPhaseMultipliesAddsSquarerootsMEKFPropagation3502702Measurement update2601851One full filter step*6104553One observation circle†168012659MGSPFPropagation3502855Measurement update3802801One full filter step7505656One observation circle1860148021SSUKFPropagation8105658Measurement update4553701One full filter step12659359One observation circle3695263033*Including 1 propagation step and 1 measurement update step.†Including several propagation steps and 1 measurement update step. In this case we take a typical value as 4 propagations for each 1 update.Table 4
Pseudocode and computational expense evaluation of the MEKF with closed-form solutions.
Algorithm×+Initializex̂0=[â0T,Δb̂0T]T=06×1, q¯̂0=[03×1T,1]T, b̂0=03×1P0A=εa2I3, P0B=εb2I3,QkA=T(σARW2+(1/3)σRRW2T2)I3QkBA=-(1/2)σRRW2T2I3,  QkB=σRRW2TI3Propagationω̂k-1=ωk-1meas-b̂k-1, ω̂k-1=|ω̂k-1|, ϕ̂k-1=ω̂k-1T651q¯̂k-1ω=[8ϕ̂k-1T,(16-ϕ̂k-12)]T/(16+ϕ̂k-12)62n̂=ω̂k-1/ω̂k-13Φk-1=I3-sinϕ̂k-1[n̂×]+(1-cosϕ̂k-1)[n̂×]2≈I3-(ϕ̂k-1-(ϕ̂k-13/6))[n̂×]+(ϕ̂k-12/2)[n̂×]24930Ψk-1=(T/2)I3-((1-cosϕ̂k-1)/ω̂k-1)[n̂k-1×]+((ϕ̂k-1-sinϕ̂k-1)/ω̂k-1)[n̂k-1×]2≈(T/2)I3-(ϕ̂k-12/2ω̂k-1)[n̂k-1×]+(ϕ̂k-13/6ω̂k-1)[n̂k-1×]25130Pk/k-1A=Φk-1Pk-1AΦk-1T+Ψk-1Pk-1BΨk-1T10881+Ψk-1Pk-1BAΦk-1T+(Ψk-1Pk-1BAΦk-1T)T+QkA5457Pk/k-1BA=Pk-1BAΦk-1T+Pk-1BΨk-1T+QkBA5448Pk/k-1B=Pk-1B+QkB3q¯̂k/k-1=q¯̂k-1⊗q¯̂k-1ω, q¯̂k/k-1=q¯̂k/k-1/∥q¯̂k/k-1∥22151b̂k/k-1=b̂k-13532712Measurement updatePk/k-1Z=HkPk/k-1XHkT+Rk=Pk/k-1A+Rk3Pk/k-1AZ=Pk/k-1AHkT=Pk/k-1APk/k-1BZ=Pk/k-1BAHkT=Pk/k-1BAKkAZ=Pk/k-1A(Pk/k-1Z)-1, KkBZ=Pk/k-1BA(Pk/k-1Z)-111458δq̅kmeas=q¯̂k/k-1-1⊗q̅kmeas1612𝓏kmeas=4qkmeas/(1+q4kmeas), γkmeas=𝓏̂kmeas54âk=KkAZγkmeas, Δb̂k=KkBZγkmeas189PkA=Pk/k-1A-KkAZ(Pk/k-1AZ)T2727PkBA=Pk/k-1BA-KkBZ(Pk/k-1AZ)T2727PkB=Pk/k-1B-Kk/k-1BZ(Pk/k-1BZ)T2727q¯̂k=q¯̂k/k-1⊗δq¯̂(âk),  q¯̂k=q¯̂k/∥q¯̂k∥22151b̂k=b̂k/k-1+Δb̂k32561851Table 5
Pseudocode and computational expense evaluation of the MGSPF algorithm.
Algorithm×+Initializex̂0=[â0T,Δb̂0T]T=06×1, q¯̂0=[03×1T,1]T,  b̂0=03×1P0A=εa2I3, P0BA=εaεbI3    m= 4S̅k-1QA=T((6σARW2-T2σRRW2)/12)I3S̅k-1QB=(σRRWT/2)I3PropagationSk-1A=Pk-1A, Sk-1BA=Pk-1BA/(Sk-1A)T25133𝒶k-1=(Sk-1A+Sk-1QA)UGS, Bk-1=(Sk-1BA-Sk-1QB)UGS054ω̂k-1=ωk-1meas-b̂k-1, ω̂k-1=|ω̂k-1|, ϕ̂k-1=ω̂k-1T651q¯̂k-1ω=[8ϕ̂k-1T,(16-ϕ̂k-12)]T/(16+ϕ̂k-12)62Fori=1:m,  ϕk-1β(i)=(ω̂k-1-βk-1(i))T3m3mq¯k-1β(i)=[8ϕk-1β(i)T,(16-ϕk-1β(i)2)]T/(16+ϕk-1β(i)2)6m2mq¯k-1α(i)=[8αk-1(i)T,(16-αk-1(i)2)]T/(16+αk-1(i)2)6m2mq¯k/k-1α(i)=(q¯̂k-1ω)-1⊗q¯k-1α(i)⊗q¯k-1β(i)32m24mαk/k-1(i)=4qk/k-1α(i)/(1+q4k/k-1α(i)) end5m1mBk/k-1=Bk-1, Zk/k-1=𝒶k/k-1âk/k-1=(1/m)𝒶k/k-1·11×m1×m, Δb̂k/k-1=Δb̂k-1=03×133(m-1)𝒶k/k-1=𝒶k/k-1-11×m∘âk/k-13mPk/k-1A=(1/m)𝒶k/k-1(𝒶k/k-1)T6+6m6(m-1)Pk/k-1BA=(1/m)Bk/k-1(𝒶k/k-1)T9+9m9(m-1)q¯̂k/k-1=q¯̂k-1⊗q¯̂k-1ω, q¯̂k/k-1=q¯̂k/k-1/∥q¯̂k/k-1∥22151b̂k/k-1=b̂k-1Form=43512855Measurement update𝓏̂k/k-1=(1/m)Zk/k-1•1m×133(m-1)Zk/k-1=Zk/k-1-11×m∘𝓏̂k/k-13mPk/k-1Z=(1/m)Zk/k-1Zk/k-1T+Rk6+6m9(m-1)+3Pk/k-1AZ=(1/m)ak/k-1Zk/k-1T9+9m9(m-1)Pk/k-1BZ=(1/m)Bk/k-1Zk/k-1T9+9m9(m-1)KkAZ=Pk/k-1A(Pk/k-1Z)-1, KkBZ=Pk/k-1BA(Pk/k-1Z)-111458δq¯kmeas=q¯k/k-1-1⊗q¯kmeas1612𝓏kmeas=4qkmeas/(1+q4kmeas), γkmeas=𝓏̂kmeas-𝓏̂k/k-154âk=KkAZγkmeas, Δb̂k=KkBZγkmeas1812PkA=Pk/k-1A-KkAZ(Pk/k-1AZ)T2727PkBA=Pk/k-1BA-KkBZ(Pk/k-1AZ)T2727PkB=Pk/k-1B-Kk/k-1BZ(Pk/k-1BZ)T2727q¯̂k=q¯̂k/k-1⊗δq¯(âk), q¯̂k=q¯̂k/∥q¯̂k∥22151b̂k=b̂k/k-1+Δb̂k3Form=43792781Figure 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 ofU is constituted with symmetrically distributed elements). (b) interdimensional symmetry (or per mutational symmetry) partly fulfilled. Define the generator point as u=[1,1,-1]T, and construct new points from u by permutation and sign-changing, altogether we can make 8 points occupying the 8 vertices of the unit cube in Figure 1. Note U 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 -U, and clearly, for an unbiased problem, choosing either U or -U is equivalent.Numerical Demonstration
Suppose that we have already obtained a 3-dimensional unbiased statea, covariance P, and its coresponding square-root matrix S. Then we are going to generate a set of sigma points with a base set U  as 𝒶=S·U. As had been claimed, theoretically we should have
(3.24)a⌢=Wm(α(1)+⋯+α(m))=0,P⌢=𝒶·ΛW·(𝒶)T=S·ST,
where a⌢ and P⌢  represent the result by numerical computation. Then we can take the norm of the residues |a⌢-0| and |diag(P⌢-P)| 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 S matrices with their diagonal elements ranked form 102~10-6.
The numerical experiment is programmed with double precision float numbers in MATLAB, and some typical results are listed in Table2. 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 10-16 of the diagonal elements of S, 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 Table5. 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 isq̅0=[0.1,0.15,0.2,1]T, 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°/s]T, 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×10-3°/s1/2, rate random walk 1.4×10-3°/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 asx̂0=[â0T,Δb̂0T]T=06×1, q¯̂0=[03×1T,1]T, and  b̂0=03×1. The initial covariance P0Xis set with the attitude-related elements P0A=(10°)2I3, and bias-related elements P0B=(0.1°/s)2I3. For MGSPF, as P0B is no longer used, we equivalently set a P0BA=P0AP0B. Specific elements in Rk and Qk are chosen through tuning, set as σr=2×10-7,σARW=4.5×10-4, and σRRW=4×10-4, respectively, for all three filters.The simulation results of attitude estimation error and and gyro bias estimation error are, respectively, illustrated in Figures2 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.Figure 2
Estimation error history of the attitude.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 Δb 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.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.



---

*Source: 507370-2010-01-04.xml*