Square-Root Adaptive Cubature Kalman Filter with Application to ...
Square-Root Adaptive Cubature Kalman Filter with Application to Spacecraft Attitude Estimation Xiaojun Tang∗ , Jianli Wei† and Kai Chen† ∗ School
of Aeronautics Northwestern Polytechnical University, Xi’an, Shannxi 710072, China [email protected] † School of Astronautics Northwestern Polytechnical University, Xi’an, Shannxi 710072, China [email protected], [email protected]
Abstract—A novel nonlinear filter called square-root adaptive cubature Kalman filter is proposed to estimate the spacecraft attitude from vector measurements. The algorithm combines an adaptive process noise estimation with the square-root cubature Kalman filter, which has a consistently improved numerical stability because all the resulting covariance matrices are guaranteed to stay positive semi-definite. The process noise estimate for efficient square-root implementation is derived. The quaternion is used to describe the spacecraft attitude kinematics, while a threedimensional generalized Rodrigues parameter is used to maintain the quaternion normalization constraint in the filter formulation. The simulation results indicate that the proposed filter provides lower attitude estimation errors with faster convergence rate than the square-root cubature Kalman filter. Index Terms—square-root adaptive cubature Kalman filter, attitude estimation, quaternion, process noise estimation.
I. I NTRODUCTION The objective of spacecraft attitude estimation is to determine the orientation of a body fixed coordinate frame with respect to a reference coordinate frame. A variety of representations can be used to describe the attitude [1], such as the direction cosine matrix (DCM) [2], Euler angles [3] and rotation vector [4] followed by quaternion [5]. The quaternion is the minimal nonsingular global attitude parameterization, which has only one redundant parameter, as opposed to the six redundant elements of the DCM. However, the quaternion must obey a normalization constraint that has to be addressed in attitude estimation algorithms. The extended Kalman filter (EKF) is the most widely applied filtering algorithm for estimating the attitude quaternion from vector measurements [6]. However, poor performance or even divergence of the EKF has led to the development of other filters. Some of these approaches adopt the EKF mechanization [7, 8]. Recently an unscented Kalman filter (UKF) has been proposed for the estimation of the rotation quaternion and demonstrated more accurate and robust performance than the EKF under large initial attitude estimation errors [9]. The UKF, which is “founded on the intuition that it is easier to approximate a probability distribution than it is to approximate an arbitrary nonlinear function” [10], generalizes elegantly to nonlinear systems without the burdensome analytic derivation or Jacobians. This approach gives the UKF
better convergence characteristics and greater accuracy than the EKF for a nonlinear system, but it also introduces some shortcomings. In particular, the UKF, which implements the scaled unscented transformation [11], has more than one free parameter, and therefore, is difficult to tune from a practical perspective. More recently, an alternative estimation algorithm called cubature Kalman filter (CKF) [12] has been proposed for general nonlinear systems. The CKF, which builds on the numerical-integration perspective of Gaussian filters [13, 14], employs a third-degree spherical-radical cubature rule to compute Gaussian-weighted integrals numerically and is a relatively new, derivative-free nonlinear filtering algorithm with improved performance over the UKF in terms of estimation accuracy, numerical stability and computational costs [12]. The performance of CKF, however, relies on the properly selected process and measurement noise statistics. For many applications, the model noise statistics are given before the filtering process and will maintain unchanged during the whole recursive process. However, this a priori statistical information is often unknown in practice, and even has time-varying characteristics. Adaptive filtering is trying to determine the statistical characteristics of nonlinear systems, and has recently gained attention. Maybeck [15] shows a maximum likelihood estimator to estimate system noise statistics. Busse et al. [16] modify Maybeck’s adaptation algorithm by properly choosing a window scale factor to control the level of the variance update. Lee and Alfriend [17] extend this adaptive scheme by introducing an innovative procedure to automatically calculate the window size using a derivative-free numerical optimization technique. The CKF has two implementation variants, standard CKF and square-root CKF (SR-CKF) [12, 18]. The SR-CKF is algebraically equivalent to the standard CKF and can consistently improve the numerical stability because all the resulting covariance matrices are guaranteed to stay positive semi-definite. In this paper, a new filtering formulation, which merges Lee and Alfriend’s adaptive process noise estimation [17] with the SR-CKF, is proposed for state estimation of general nonlinear systems. The resulting algorithm is thus referred to as square-root adaptive cubature Kalman filter (SR-ACKF). Furthermore, the process noise estimate for efficient square-
1406
root implementation is derived. As a simulation, the SR-ACKF is then applied to spacecraft attitude estimation from vector observations. The rest of this paper is organized as follows. The detailed derivation of square-root adaptive cubature Kalman filter is provided in Section II. Section III introduces the attitude kinematics and sensor models. Section IV describes the implementation details of the SR-ACKF algorithm for spacecraft attitude estimation. Section V presents the simulation results to compare the performance of the filters with each other. Finally, some concluding remarks are given in Section VI. II. S QUARE - ROOT ADAPTIVE CUBATURE K ALMAN FILTER In this section, we give the derivation of SR-ACKF for nonlinear state estimation. A. Process noise covariance estimation An attractive approach to the adaptive Kalman filtering problem is covariance matching, in which the process noise covariance Pv𝑘 and observation covariance Pn𝑘 themselves are adapted as measurements evolve with time. However, adaptive filtering algorithms that try to estimate both the system noise and the observation noise simultaneously are not robust. Since the measurement noise statistics are relatively well known compared to the system errors, we will only estimate process noise covariance Pv𝑘 adaptively in this work. Following Maybeck’s adaptation approach, the estimate for the process noise covariance is given by [15] −
ˆ P∗v𝑘 = Δx𝑘 Δx𝑇𝑘 + P− x𝑘 − Px𝑘 − Pv𝑘
(1)
ˆ𝑘 − ˆ where Δx𝑘 ≡ x x− 𝑘 is the state residual and represents the difference between the state estimate before the measurement update and after the measurement update. If this residual has a large value, then it indicates that the future state prediction is not accurate enough. P− x𝑘 and Px𝑘 denote a priori and a ˆ− posteriori state covariance estimate. P v𝑘 is the current expected process noise covariance. It is noteworthy that Maybeck’s estimator for system noise statistics is unbiased, i.e., the bias is assumed to equal to zero. Busse et al. extend Maybeck’s adaptation algorithm by combining the measure of the process noise P∗v𝑘 with the ˆ− current expected estimate P v𝑘 in a moving average [16] ) 1 ( ∗ ˆ− ˆ− ˆ v𝑘 = P Pv𝑘 − P P v𝑘 + v𝑘 𝐿𝑣
(2)
where 𝐿𝑣 is the window size that controls the level of expected update change. If 𝐿𝑣 is small, then each update is weighted heavily, but if 𝐿𝑣 is large, then each update has a small effect. The performance of the adaptive routine is very sensitive to the selection of 𝐿𝑣 , and thus should be properly selected for each application.
ˆ v𝑘 , in Eq. (2) can be The current process noise estimate, P rewritten for efficient square-root implementation as ( ) ) 𝐿𝑣 − 2 ˆ − 1 ( 1 ˆ Δx𝑘 Δx𝑇𝑘 + P− Pv𝑘 + Pv𝑘 = x𝑘 − Px𝑘 𝐿𝑣 𝐿𝑣 ( ) (3) ) − 𝐿 1 ( − 2 𝑣 𝑇 2 𝑇 ˆ Δx𝑘 Δx𝑘 + K𝑘 Py𝑘 K𝑘 Pv𝑘 + = 𝐿𝑣 𝐿𝑣 where 1 is a straightforward substitution of P∗v𝑘 given in Eq. (1), and 2 uses the Kalman state covariance update equation. B. Automated calculation of window scale factor The conventional approach to determine the window scale factor is an ad hoc process involving a considerable amount of trial-and-error to obtain near-optimal estimation results. Numerical optimization algorithms, however, can be used to replace the tedious, repetitive trial-and-error technique. The Downhill Simplex algorithm, which is a derivativefree numerical optimization technique, has been used to tune the parameters of the process noise covariance [19]. Lee and Alfriend use this method to automatically determine the window scale factor for nonlinear adaptive estimator [17]. This method will also be used in this paper. In order to apply the numerical optimization algorithm to determining the scale factor, a scalar objective function has to be constructed. A commonly used objective function in MonteCarlo performance simulation is the overall performance index 𝐽, which is defined as )12 ( 𝐿𝑚𝑐 1 ∑ 𝐽 2 (𝐿𝑣 ) (4) 𝐽 ≡ 𝐿𝑚𝑐 𝑖=1 𝑘,𝑖 where 𝐿𝑚𝑐 denotes the number of Monte-Carlo runs, 𝑖 represents the 𝑖th simulation run, and 𝐽𝑘,𝑖 (𝐿𝑣 ) is defined by )12 ( 𝐿𝑜𝑠 1 ∑ 𝐽𝑘,𝑖 (𝐿𝑣 ) ≡ ∥g𝑘,𝑖 ∥2 (5) 𝐿𝑜𝑠 𝑘=1
where 𝐿𝑜𝑠 is the length of entire observation sequences. The innovation g𝑘 = y𝑘 − ˆy− 𝑘 represents the difference between the actual and the predicted observations. C. Square-root adaptive cubature Kalman filter The general discrete-time nonlinear system model is given by x𝑘 = f(x𝑘−1 , v𝑘−1 , u1𝑘−1 ) y𝑘 = h(x𝑘 , n𝑘 , u2𝑘 )
(6a) (6b)
where x𝑘 ∈ ℝ𝑛𝑥 denotes the 𝑛𝑥 × 1 state vector and y𝑘 ∈ ℝ𝑛𝑦 the 𝑛𝑦 × 1 output observation vector. v𝑘 ∈ ℝ𝑛𝑣 is the 𝑛𝑣 × 1 state noise process vector and n𝑘 ∈ ℝ𝑛𝑛 the 𝑛𝑛 × 1 measurement noise vector. u1𝑘 ∈ ℝ𝑛𝑢1 is the 𝑛𝑢1 × 1 state input vector and u2𝑘 ∈ ℝ𝑛𝑢2 the 𝑛𝑢2 × 1 observation input vector. The mappings f : ℝ𝑛𝑥 × ℝ𝑛𝑣 × ℝ𝑛𝑢1 −→ ℝ𝑛𝑥 and h : ℝ𝑛𝑥 × ℝ𝑛𝑛 × ℝ𝑛𝑢2 −→ ℝ𝑛𝑦 represent the deterministic process and measurement models. It is assumed that v𝑘 and
1407
n𝑘 are uncorrelated zero-mean Gaussian noise processes with covariances given by Pv𝑘 and Pn𝑘 . The state random variable for SR-ACKF is redefined as the concatenation of the original state and the process and observation noise random variables [20]: ⎡ ⎤ ⎡ 𝑥⎤ x𝑘 x𝑘 (7) x𝑎𝑘 ≡ ⎣x𝑣𝑘 ⎦ ≡ ⎣v𝑘 ⎦ x𝑛𝑘 n𝑘
(b) Time update equations: ) ( 𝑥 − 𝑣 (𝑖), 𝒳𝑘−1 (𝑖), u1𝑘−1 𝒳𝑘𝑥 (𝑖) = f 𝒳𝑘−1 ˆx− 𝑘 =
𝑖=1
Π𝒳 𝑥− 𝑘
The effective dimension of this augmented state random variable is now 𝑛𝑎 = 𝑛𝑥 + 𝑛𝑣 + 𝑛𝑛 . In a similar manner the square root of augmented state covariance matrix is built up from the individual covariance of matrices of x, v and n [20]: ⎤ ⎡ 0 0 Sx𝑘 0 ⎦ (8) S𝑎𝑘 = ⎣ 0 Sv𝑘 0 0 S n𝑘
where the weights are given by [12] 1 𝑤𝑖𝑛𝑎 = , 𝑖 = 1, 2, . . . , 2𝑛𝑎 2𝑛𝑎 (c) Measurement update equations: ) ( − 𝑛 𝒴𝑘 (𝑖) = h 𝒳𝑘𝑥 (𝑖), 𝒳𝑘−1 (𝑖), u2𝑘
Π𝒴𝑘
𝑤𝑖𝑛𝑎 𝒴𝑘 (𝑖)
⎡ √ ( )𝑇 ⎤𝑇 𝑤1𝑛𝑎 𝒴𝑘 (1) − ˆy− 𝑘 ⎢ )𝑇 ⎥ ⎥ ⎢ √ 𝑛 ( − 𝑎 ⎥ ⎢ ˆ 𝒴 (2) − y 𝑤 𝑘 𝑘 2 ⎥ ⎢ = ⎢ ⎥ .. ⎥ ⎢ ⎥ ⎢ . ⎣√ ( )𝑇 ⎦ 𝑛𝑎 𝒴𝑘 (2𝑛𝑎 ) − ˆy− 𝑤2𝑛 𝑘 𝑎
Sy𝑘 = tria (Π𝒴𝑘 )
Px− y = Π𝒳 𝑥− Π𝑇𝒴𝑘 𝑘 𝑘 ) ( 𝑘 K𝑘 = Px𝑘 y𝑘 /S𝑇y𝑘 /Sy𝑘 ) ( g𝑘 = y𝑘 − h ˆx− 𝑘 , 0, u2𝑘 ˆx𝑘 =
where 𝐸[⋅] denotes expectation operator, Pv0 and Pn0 are the initial process and observation noise covariances. 2) for 𝑘 = 1, . . . , ∞,
Sx𝑘
(12b)
ˆx− 𝑘
+ K𝑘 g𝑘 ]) ([ = tria Π𝒳 𝑥− − K𝑘 Π𝒴𝑘 𝑘
(12c) (12d)
(13)
(14a) (14b)
(14c)
(14d) (14e) (14f) (14g) (14h) (14i)
(d) Process noise covariance estimation: ] [√ √ [ ] 𝐿𝑣 − 2 ˆ 1 Sv𝑘−1 , Πv𝑘 = K𝑘 g𝑘 , Sy𝑘 𝐿𝑣 𝐿𝑣
(a) Calculate the cubature points: 𝑖 = 1, 2, . . . , 2𝑛𝑎 (10)
(15a) ˆv = tria (Πv ) S 𝑘 𝑘 ⎤ ⎡ Sx𝑘 0 0 ˆv S𝑎𝑘 = ⎣ 0 S 0 ⎦ 𝑘 0 0 Sn𝑘
⎡
(11)
and ℰ 𝑛𝑎 is the standard cubature points set for a 𝑛𝑎 dimensional state space. The approach to calculate ℰ 𝑛𝑎 is presented in [12] and is not shown here for brevity.
2𝑛𝑎 ∑ 𝑖=1
]) ( [ ˆ x0 )(x0 − ˆx0 )𝑇 , x0 = 𝐸[x0 ] , Sx0 = chol 𝐸 (x0 − ˆ ˆv = chol (Pv ) , Sn = chol (Pn ) , S 0 0 0 0 ⎤ ⎡ 0 0 S x0 [ ] 𝑇 ˆv ˆ x𝑎0 = 𝐸[x𝑎0 ] = ˆ x𝑇0 , 0, 0 , S𝑎0 = ⎣ 0 S 0 ⎦ 0 0 0 Sn0 (9)
⎤ 𝑥 (𝑖) 𝒳𝑘−1 𝑎 𝑣 (𝑖)⎦ (𝑖) ≡ ⎣𝒳𝑘−1 𝒳𝑘−1 𝑛 𝒳𝑘−1 (𝑖)
)
𝑘
ˆy− 𝑘 =
(12a)
⎡ √ ( − )𝑇 ⎤𝑇 𝑤1𝑛𝑎 𝒳𝑘𝑥 (1) − ˆx− 𝑘 ⎢ )𝑇 ⎥ ⎥ ⎢ √ 𝑛 ( − − 𝑥 𝑎 ⎥ ⎢ 𝑤2 𝒳𝑘 (2) − ˆx𝑘 ⎥ ⎢ = ⎢ ⎥ .. ⎥ ⎢ ⎥ ⎢ . ⎣√ ( − )𝑇 ⎦ − 𝑛𝑎 𝑤2𝑛𝑎 𝒳𝑘𝑥 (2𝑛𝑎 ) − ˆ x𝑘
S− x𝑘 = tria Π𝒳 𝑥−
1) Initialization:
where
−
𝑤𝑖𝑛𝑎 𝒳𝑘𝑥 (𝑖)
(
where Sv and Sn are the square root of process and observation noise covariances, defined by SS𝑇 ≡ P. The SR-ACKF essentially propagates square-root factors of the predictive and posterior error covariances directly, thereby avoiding matrix square-rooting operations in each filtering cycle. Specifically speaking, the SR-ACKF uses the leastsquares method for the Kalman gain to avoid computing a matrix inversion explicitly, and matrix triangular factorizations or triangularizations (for instance, the QR decomposition) for covariance updates to compute a triangular square-root factor of the covariance. The entire algorithm is presented as follows:
𝑎 ˆ𝑎𝑘−1 +S𝑎𝑘−1 ℰ 𝑛𝑎 (𝑖), (𝑖) = x 𝒳𝑘−1
2𝑛𝑎 ∑
(15b) (15c)
It is noteworthy here that chol( ⋅ ) denotes a Cholesky decomposition of a matrix where a lower triangular Cholesky factor is returned, and tria( ⋅ ) is a general matrix triangularization algorithm which also generates a lower triangular
1408
matrix. The innovation g𝑘 is calculated using the actual measurement model and the propagated state, which incorporates the second-order terms of the measurement model into the state estimate update [21]. This is the most general form of the SR-ACKF. When the system and observation noises are assumed purely additive, the implementations of SR-ACKF need not augmented with the noise variables, thus reducing the number of cubature points and computational complexity. III. ATTITUDE KINEMATICS AND SENSOR MODELS In this section, we briefly review some preliminaries such as the quaternion attitude representation, gyro and attitude-vector models. The quaternion is a four-dimensional vector in which the scalar portion of the quaternion is the fourth element, defined as [ ] 𝝔 (16) q ≡ 𝑞4 with 𝝔 ≡ [𝑞1 , 𝑞2 , 𝑞3 ]
𝑇
= 𝒆 sin(𝜗/2)
𝑞4 = cos(𝜗/2)
(17a) (17b)
where 𝒆 is the unit Euler axis and 𝜗 is the rotation angle. Since a four-dimensional vector is used to describe three dimensions, the quaternion components cannot be independent of each other. The quaternion satisfies a normalization constraint given by q𝑇 q = ∥q∥2 = 1, which ensures that the DCM is orthonormal and is analogous to the fact that the Euler axis 𝒆 is a unit vector. The DCM (also called attitude matrix) is related to the quaternion by ) ( (18) 𝐴(q) = 𝑞42 − ∥𝝔∥2 𝐼3×3 + 2𝝔𝝔𝑇 − 2𝑞4 [𝝔×] where 𝐼3×3 is a 3 × 3 identity matrix and [𝝔×] is a crossproduct matrix since three-component vector cross-product operation 𝒂 × 𝒄 = [𝒂×]𝒄, which is defined by ⎤ ⎡ 0 −𝑞3 𝑞2 0 −𝑞1 ⎦ (19) [𝝔×] ≡ ⎣ 𝑞3 −𝑞2 𝑞1 0 An advantage of the quaternion is that successive rotations can be accomplished using quaternion multiplication, which can be written using [9] 𝐴(q)𝐴(q′ ) = 𝐴(q ⊗ q′ )
(20)
where the ⊗ vector operator denotes the quaternion multiplication, which represents the matrix operation q ⊗ q′ = [q⊗]q′ . The quaternion multiplication operator matrix [q⊗] is defined by ⎤ ⎡ 𝑞4 𝑞3 −𝑞2 𝑞1 ⎢−𝑞3 𝑞4 𝑞1 𝑞2 ⎥ ⎥ (21) [q⊗] ≡ ⎢ ⎣ 𝑞2 −𝑞1 𝑞4 𝑞3 ⎦ −𝑞1 −𝑞2 −𝑞3 𝑞4 Also, the inverse quaternion is defined by [ ] −𝝔 q−1 ≡ 𝑞4
(22)
Note that 𝐴(q−1 ) = 𝐴𝑇 (q) and q ⊗ q−1 = [0, 0, 0, 1]𝑇 , which is the identity quaternion. The quaternion kinematics equation is given by 1 (23) q˙ = Ω(𝝎)q 2 where 𝝎 = [𝜔1 , 𝜔2 , 𝜔3 ]𝑇 is the three-component angular rate vector and ] [ −[𝝎×] 𝝎 (24) Ω(𝝎) ≡ −𝝎 𝑇 0 A discrete-time approximation to the solution of Eq. (23) is given by [9] (25) q𝑘+1 = Λ(𝝎𝑘 )q𝑘 where
[ Λ(𝝎𝑘 ) ≡
𝑈𝑘
( 𝝍𝑘
]
)
(26a) −𝝍𝑘𝑇 cos ∥𝝎𝑘2∥Δ𝑡 ⎛ ( ∥𝝎 ∥Δ𝑡 ) ⎞ 𝑘 sin 2 ⎠𝝎 𝑘 𝝍𝑘 ≡ ⎝ (26b) ∥𝝎𝑘 ∥ ) ( ∥𝝎𝑘 ∥Δ𝑡 𝐼3×3 − [𝝍𝑘 ×] 𝑈𝑘 ≡ cos (26c) 2 and Δ𝑡 ≡ 𝑡𝑘+1 − 𝑡𝑘 is the sampling interval in the gyro output. Herein, we adopt a three-component generalized Rodrigues parameter (GRP) to represent the unconstrained attitude parameter, which is defined by [22] 𝜹𝝔 (27) 𝜹𝒑 ≡ 𝑙 ℎ + 𝛿𝑞4 where ℎ is a parameter from 0 to 1, 𝑙 is a scale factor and the error-quaternion 𝜹q is defined by 𝜹q ≡ q ⊗ q−1 𝑟
(28)
where q𝑟 denotes the reference quaternion which usually is ˆ and 𝜹q ≡ [𝜹𝝔𝑇 , 𝛿𝑞4 ]𝑇 . For small the estimated quaternion q angles the vector part of error-quaternion is approximately equal to half error angles [1]. We will choose 𝑙 = 2(ℎ + 1) so that ∥𝜹𝒑∥ is equal to 𝜗 for small errors. The inverse transformation from 𝜹𝒑 to 𝜹q is given by [9] √ −ℎ∥𝜹𝒑∥2 + 𝑙 𝑙2 + (1 − ℎ2 )∥𝜹𝒑∥2 (29a) 𝛿𝑞4 = 𝑙2 + ∥𝜹𝒑∥2 𝜹𝝔 = 𝑙−1 (ℎ + 𝛿𝑞4 )𝜹𝒑 (29b) A common sensor that measures the angular rate is a rateintegrating gyro. For this sensor, a widely used three-axis continuous-time model is given by [23] ˜ = 𝝎 + 𝜷 + 𝜼𝑣 𝝎 𝜷˙ = 𝜼𝑢
(30a) (30b)
˜ is the measured rate, 𝜷 is the gyro drift, and 𝜼𝑣 and where 𝝎 𝜼𝑢 are independent zero-mean Gaussian white-noise processes with ] [ (31a) 𝐸 𝜼𝑣 (𝑡)𝜼𝑣𝑇 (𝜏 ) = 𝜎𝑣2 𝛿(𝑡 − 𝜏 )𝐼3×3 [ ] 𝐸 𝜼𝑢 (𝑡)𝜼𝑢𝑇 (𝜏 ) = 𝜎𝑢2 𝛿(𝑡 − 𝜏 )𝐼3×3 (31b)
1409
where 𝛿(⋅) is the Dirac-delta function. The parameters of 𝜎𝑣 and 𝜎𝑢 can be experimentally obtained using frequency response data from the gyro outputs with units of rad/s1/2 and rad/s3/2 . For simulation purposes, discrete-time gyro measurements can be generated using the following recursive equations [24]: ( 2 )12 1 2 1 𝜎𝑣 ˜ 𝑘+1 = 𝝎𝑘+1 + (𝜷𝑘+1 + 𝜷𝑘 ) + + 𝜎 Δ𝑡 N𝑣 𝝎 2 Δ𝑡 12 𝑢 (32a) 1
𝜷𝑘+1 = 𝜷𝑘 + 𝜎𝑢 Δ𝑡 2 N𝑢
with
˜𝑖 denotes the 𝑖th 3 × 1 measurement vector, 𝒓𝑖 is the where 𝒃 𝑖th known 3 × 1 reference vector, and sensor error vector 𝝂𝑖 is Gaussian which satisfies 𝐸[𝝂𝑖 ] = 0 ] [ 𝐸 𝝂𝑖 𝝂𝑖𝑇 = 𝜎𝑖2 𝐼3×3 Multiple vector measurements can be given by ⎡˜ ⎤ ⎡ ⎡ ⎤ ⎤ 𝒃1 𝐴(q)𝒓1 𝝂1 ˜2 ⎥ ⎢𝒃 ⎢ 𝐴(q)𝒓2 ⎥ ⎢ 𝝂2 ⎥ ⎢ ⎥ ⎢ ⎢ ⎥ ⎥ y𝑘 = ⎢ . ⎥ = ⎢ ⎥ + ⎢ .. ⎥ .. ⎣ .. ⎦ ⎣ ⎣ . ⎦ ⎦ . ˜𝑁 𝐴(q)𝒓𝑁 𝝂𝑁 𝒃 𝑘
(34a) (34b)
𝑘
where 𝑁 is the number of measurement vectors provided by the attitude sensors. The measurement noise covariance in this case has a covariance of ]) ([ 2 (36) 𝐼3×3 Pn𝑘 = diag 𝜎12 𝐼3×3 , 𝜎22 𝐼3×3 , . . . , 𝜎𝑁 where diag( ⋅ ) denotes a diagonal matrix of appropriate dimension. IV. ATTITUDE ESTIMATION USING SQUARE - ROOT ADAPTIVE CUBATURE K ALMAN FILTER It is well known that when using a quaternion attitude representation, any updates produced from the Kalman type estimator can violate the quaternion unit-norm constraint. This can be alleviated by utilizing the quaternion representation for state propagation, but transforming this to an unconstrained attitude representation for the update.
[
𝑖 = 1, 2, . . . , 2𝑛𝑎
(38a) (38b)
where 𝜹 𝒑ˆ𝑘 denotes the estimated attitude error and 𝜷ˆ𝑘 represents the estimated gyro bias. 𝒳𝑘𝛿𝑝 denotes the attitude error part and 𝒳𝑘𝛽 represents gyro bias part. At each timestep, before an update is performed, the error-quaternion estimate, denoted by 𝜹ˆ q𝑘 (𝑖), can be transformed from the corresponding cubature points through Eq. (29). Then, the following quaternions are computed: ˆ𝑘, ˆ 𝑘 (𝑖) = 𝜹ˆ q𝑘 (𝑖) ⊗ q q
𝑖 = 1, 2, . . . , 2𝑛𝑎
(39)
The cubature-point quaternions in Eq. (39) are propagated using Eq. (25), with ( ) ˆ− ˆ 𝑘 (𝑖) q ˆ 𝑘 (𝑖), 𝑖 = 1, 2, . . . , 2𝑛𝑎 (40) q 𝑘+1 (𝑖) = Λ 𝝎 where the estimated angular rates are given by [9] ˜ 𝑘 − 𝒳𝑘𝛽 (𝑖), ˆ 𝑘 (𝑖) = 𝝎 𝝎
𝑖 = 1, 2, . . . , 2𝑛𝑎
(41)
The propagated error-quaternions are then computed using the definition of error-quaternion ( − )−1 ˆ− ˆ 𝑘+1 , 𝑖 = 1, 2, . . . , 2𝑛𝑎 (42) 𝜹ˆ q− 𝑘+1 (𝑖) = q 𝑘+1 (𝑖) ⊗ q with
(35)
] 𝒳𝑘𝛿𝑝 (𝑖) , 𝒳𝑘𝛽 (𝑖) ] [ 𝜹 𝒑ˆ ˆx𝑘 ≡ ˆ 𝑘 𝜷𝑘
𝒳𝑘𝑥 (𝑖) ≡
(32b)
where the subscript 𝑘 denotes the 𝑘th time-step, Δ𝑡 is the gyro sampling interval, and N𝑣 and N𝑢 are independent zero-mean Gaussian white-noise processes with covariance each given by the identity matrix. Typical vector observations on-board spacecrafts are the Earth magnetic field, the direction to the Sun, to the Moon, or to other celestial bodies. In general, the reference vectors, 𝒓, are known quite accurately from tables or almanacs, while the body vectors, 𝒃, are obtained from measurements corrupted by errors. As a result, the spacecraft attitude-vector measurement will be assumed to be in the form known as the QUEST measurement model [25], which for a single sensor is given by ˜𝑖 = 𝐴(q)𝒓𝑖 + 𝝂𝑖 (33) 𝒃
𝑘
We start the spacecraft attitude estimation by defining the following augmented cubature-point state vectors: ⎡ 𝑥 ⎤ 𝒳𝑘 (𝑖) (37) 𝒳𝑘𝑎 (𝑖) ≡ ⎣ 𝒳𝑘𝑣 (𝑖) ⎦ , 𝑖 = 1, 2, . . . , 2𝑛𝑎 𝒳𝑘𝑛 (𝑖)
) ( ˆ− ˆ𝑘 ˜ 𝑘 − 𝜷ˆ𝑘 q q 𝑘+1 = Λ 𝝎
(43)
The gyro bias is propagated using [9] − = 𝜷ˆ𝑘 𝜷ˆ𝑘+1
(44)
Therefore, the propagated cubature-point states, i.e., GRPs 𝛿𝑝 𝛽 (𝑖) and gyro biases 𝒳𝑘+1 (𝑖), are formed through Eqs. 𝒳𝑘+1 (27) and (44) with the square-root factor of estimated process noise covariance obtained from Eq. (15b). The predicted state mean and covariance are then computed using Eqs. (12b) and (12d). The propagated quaternions are used to calculate the cubature-point observations using Eq. (35), with ) ⎤ ⎡ ( − ˆ 𝑘+1 (𝑖))𝒓1 𝐴(q − ⎢𝐴 q ⎥ ⎢ ˆ 𝑘+1 (𝑖) 𝒓2 ⎥ 𝒴𝑘+1 (𝑖) = ⎢ ⎥ + 𝒳𝑘𝑛 (𝑖), 𝑖 = 1, 2, . . . , 2𝑛𝑎 .. ⎣ ⎦ ( − . ) ˆ 𝑘+1 (𝑖) 𝒓𝑁 𝐴 q (45) The observation mean and covariance are calculated using Eqs. (14b) and (14d) with measurement noise covariance given by Eq. (36). After the state estimate and covariance have been updated using Eqs. (14h) and (14i) from the most
1410
recent measurements, the actual attitude quaternion must be determined from the updated error-quaternion using
10
ˆ− ˆ 𝑘+1 = 𝜹ˆ q𝑘+1 ⊗ q q 𝑘+1
10
3
SR−CKF SR−ACKF
(46) RMSE−Attitude ( ° )
2
where 𝜹ˆ q𝑘+1 is transformed from the updated GRP, 𝜹 𝒑ˆ𝑘+1 , through Eq. (29). In this manner, the attitude quaternion estimation is performed without violation of the norm constraint. V. S IMULATION RESULTS In this section, a simulation with one vector measurement and three angular rate measurements is used to determine the attitude of a rotating spacecraft. The performance comparison of the SR-CKF and SR-ACKF is demonstrated through the simulation. For a fair comparison, a total of 100 independent Monte-Carlo runs are made and the root-mean suqare error (RMSE) of the attitude is chosen as the performance metric. For each run a different realization of both the process and observation noises is generated. The window scale factor for the SR-ACKF obtained from the Downhill Simplex algorithm is 86. Also, the corresponding parameters for the GRP are set to ℎ = 1 with 𝑙 = 4. All measurements are assumed to be sampled every 10 s and the total time of the simulation is 12 h. The attitude measurements are taken from a three-axis magnetometer (TAM) utilizing the vector measurement model previously discussed with a standard deviation of 50 nT. The gyro angular rate measurements are synthesized using Eq. (32) with gyro noise parameters 𝜎𝑣 = 3.1026 × 10−7 rad/s1/2 and 𝜎𝑢 = 9.0358 × 10−10 rad/s3/2 , and an initial bias of 0.6 ∘ /h for each axis. In order to stress the filters, the simulation run begins with the initial attitude errors of −60∘ , 70∘ and 20∘ for each axis, and the initial bias estimate of [0, 20, 0]𝑇 ∘ /h, respectively. The three attitude the initial covariance are each set to ( error parts)of 2 ∘ 50 , i.e., 50×(𝜋/180) rad2 . The three gyro(bias parts of the initial covariance are each set to 20 ∘ /h, i.e., 20 × (𝜋/(180 × )2 3600)) (rad/s)2 . The discrete process noise covariance for the SR-CKF is given by [9] ) ( 1 2 2) ] [( 2 1 2 3 𝜎𝑣 Δ𝑡 ( 1 +2 3 𝜎2𝑢)Δ𝑡 𝐼3×3 (2 𝜎2𝑢 Δ𝑡) 𝐼3×3 (47) Pv𝑘 = 𝐼3×3 𝜎𝑢 Δ𝑡 𝐼3×3 2 𝜎𝑢 Δ𝑡
1
10
0
10
−1
10
−2
10
0
2
4
Fig. 1.
6 Time (h)
8
10
12
RMSE in attitude
VI. C ONCLUSIONS In this paper a new approach for accurate and efficient state estimation of general nonlinear systems using the squareroot adaptive cubature Kalman filter is proposed. The ability to capture second order nonlinearities and the adaptive process noise covariance estimation of the proposed filter make it an attractive alternative to other nonlinear filters for spacecraft attitude estimation, which is inherently nonlinear. In order to avoid the quaternion normalization constraint, a three-component generalized Rodrigues parameter is used to represent the attitude for the update in the filer formulation. The performance of this novel filter in spacecraft attitude estimation is shown through the simulation.
A plot of the RMSE in attitude for the SR-CKF and SRACKF is shown in Fig. 1. As can be seen, the convergence performance of the SR-ACKF is far superior to that of the SRCKF. The SR-CKF converges slowly and takes almost 10 h to achieve a converged solution, while the SR-ACKF takes about 5 h. Furthermore, the errors of the SR-CKF are much higher than that of the SR-ACKF although they finally converge to similar value. The overall results obtained for the SR-CKF and SR-ACKF from the simulation clearly indicate that the SR-ACKF achieved the near-optimal performance in terms of the unknown uncertainties and neglected modeling errors. The rational behind this is that (1) the process noise statistics are adaptively tuned, and (2) the window scale factor is efficiently optimized by the Downhill Simplex algorithm.
1411
R EFERENCES [1] Shuster, M. D., “A Survey of Attitude Representations”, Journal of the Astronautical Sciences, Vol. 41, No. 4, October-December 1993, pp. 439-517. [2] Oshman, Y., and Markley, F. L., “Minimal-Parameter Attitude Matrix Estimation from Vector Observations”, Journal of Guidance, Control, and Dynamics, Vol. 21, No. 4, July-August 1998, pp. 595-602. [3] Farrell, J. L., “Attitude Determination by Kalman Filter”, Automatica, Vol. 6, No. 5, May 1970, pp. 419-430. [4] Pittelkau, M. E., “Rotation Vector in Attitude Estimation”, Journal of Guidance, Control, and Dynamics, Vol. 26, No. 6, November-December 2003, pp. 855-860. [5] Hamilton, W. R., Elements of Quaternions, Longmans, Green and Co., London, England, 1866. [6] Lefferts, E. J., Markley, F. L., and Shuster, M. D., “Kalman Filtering for Spacecraft Attitude Estimation”, Journal of Guidance, Control, and Dynamics, Vol. 5, No. 5, September-October 1982, pp. 417-429. [7] Psiaki, M. L., “Backward-Smoothing Extended Kalman Filter”, Journal of Guidance, Control, and Dynamics, Vol. 28, No. 5, September-October 2005, pp. 885-894.
[8] Zanetti, R., Majji, M., Bishop, R. H., and Mortari, D., “Norm-Constrained Kalman Filtering”, Journal of Guidance, Control, and Dynamics, Vol. 32, No. 5, SeptemberOctober 2009, pp. 1458-1465. [9] Crassidis, J. L., and Markley, F. L., “Unscented Filtering for Spacecraft Attitude Estimation”, Journal of Guidance, Control, and Dynamics, Vol. 26, No. 4, July-August 2003, pp. 536-542. [10] Julier, S. J., and Uhlmann, J. K., “Unscented Filtering and Nonlinear Estimation”, Proceedings of the IEEE, Vol. 92, No. 3, March 2004, pp. 401-422. [11] Julier, S. J., “The Scaled Unscented Transformation”, Proceedings of the American Control Conference, Anchorage, AK, May 2002, pp. 1108-1114. [12] Arasaratnam, I., and Haykin, S., “Cubature Kalman Filters”, IEEE Transactions on Automatic Control, Vol. 54, No. 6, June 2009, pp. 1254-1269. [13] Ito, K., and Xiong, K., “Gaussian Filters for Nonlinear Filtering Problems”, IEEE Transactions on Automatic Control, Vol. 45, No. 5, May 2000, pp. 910-927. [14] Wu, Y., Hu, D., Wu, M., and Hu, X., “A NumericalIntegration Perspective on Gaussian Filters”, IEEE Transactions on Signal Processing, Vol. 54, No. 8, August 2006, pp. 2910-2921. [15] Maybeck, P., Stochastic Models, Estimation, and Control, Vol. 2, Academic Press, New York, 1972. [16] Busse, F. D., How, J. P., and Simpson, J., “Demonstration of Adaptive Extended Kalman Filter for Low Earth Orbit Formation Estimation Using CDGPS”, Institute of Navigation GPS Meeting, Portland, OR, September 2002. [17] Lee, D.-J., and Alfriend, K. T., “Adaptive Sigma Point Filtering for State and Parameter Estimation”, AIAA/AAS Astrodynamics Specialist Conference and Exhibit, Providence, RI, August 2004, AIAA-2004-5101. [18] Arasaratnam, I., and Haykin, S., “Square-Root Quadrature Kalman Filtering”, IEEE Transactions on Signal Processing, Vol. 56, No. 6, June 2008, pp. 2589-2593. [19] Powell, T. D., “Automated Tuning of an Extended Kalman Filter Using the Downhill Simplex Algorithm”, Journal of Guidance, Control, and Dynamics, Vol. 25, No. 5, September-October 2002, pp. 901-908. [20] Wan, E. A., and van der Merwe, R., Kalman Filtering and Neural Networks, John Wiley & Sons, New York, NY, September 2001, Chap. 7. [21] Perea, L., and Elosegui, P., “New State Update Equation for the Unscented Kalman Filter”, Journal of Guidance, Control, and Dynamics, Vol. 31, No. 5, SeptemberOctober 2008, pp. 1500-1503. [22] Schaub, H., and Junkins, J. L., “Stereographic Orientation Parameters for Attitude Dynamics: A Generalization of the Rodrigues Parameters”, Journal of the Astronautical Sciences, Vol. 44, No. 1, January-March 1996, pp. 1-20. [23] Farrenkopf, R. L., “Analytic Steady-State Accuracy Solutions for Two Common Spacecraft Attitude Estimators”, Journal of Guidance and Control, Vol. 1, No. 4, July-
August 1978, pp. 282-284. [24] Crassidis, J. L, “Sigma-Point Kalman Filtering for Integrated GPS and Inertial Navigation”, AIAA Guidance, Navigation, and Control Conference, San Francisco, CA, August 2005, AIAA-2005-6052. [25] Shuster, M.D., and Oh, S. D., “Three-Axis Attitude Determination from Vector Observations”, Journal of Guidance and Control, Vol. 4, No. 1, January-February 1981, pp. 70-77.
1412
of Aeronautics Northwestern Polytechnical University, Xi’an, Shannxi 710072, China [email protected] † School of Astronautics Northwestern Polytechnical University, Xi’an, Shannxi 710072, China [email protected], [email protected]
Abstract—A novel nonlinear filter called square-root adaptive cubature Kalman filter is proposed to estimate the spacecraft attitude from vector measurements. The algorithm combines an adaptive process noise estimation with the square-root cubature Kalman filter, which has a consistently improved numerical stability because all the resulting covariance matrices are guaranteed to stay positive semi-definite. The process noise estimate for efficient square-root implementation is derived. The quaternion is used to describe the spacecraft attitude kinematics, while a threedimensional generalized Rodrigues parameter is used to maintain the quaternion normalization constraint in the filter formulation. The simulation results indicate that the proposed filter provides lower attitude estimation errors with faster convergence rate than the square-root cubature Kalman filter. Index Terms—square-root adaptive cubature Kalman filter, attitude estimation, quaternion, process noise estimation.
I. I NTRODUCTION The objective of spacecraft attitude estimation is to determine the orientation of a body fixed coordinate frame with respect to a reference coordinate frame. A variety of representations can be used to describe the attitude [1], such as the direction cosine matrix (DCM) [2], Euler angles [3] and rotation vector [4] followed by quaternion [5]. The quaternion is the minimal nonsingular global attitude parameterization, which has only one redundant parameter, as opposed to the six redundant elements of the DCM. However, the quaternion must obey a normalization constraint that has to be addressed in attitude estimation algorithms. The extended Kalman filter (EKF) is the most widely applied filtering algorithm for estimating the attitude quaternion from vector measurements [6]. However, poor performance or even divergence of the EKF has led to the development of other filters. Some of these approaches adopt the EKF mechanization [7, 8]. Recently an unscented Kalman filter (UKF) has been proposed for the estimation of the rotation quaternion and demonstrated more accurate and robust performance than the EKF under large initial attitude estimation errors [9]. The UKF, which is “founded on the intuition that it is easier to approximate a probability distribution than it is to approximate an arbitrary nonlinear function” [10], generalizes elegantly to nonlinear systems without the burdensome analytic derivation or Jacobians. This approach gives the UKF
better convergence characteristics and greater accuracy than the EKF for a nonlinear system, but it also introduces some shortcomings. In particular, the UKF, which implements the scaled unscented transformation [11], has more than one free parameter, and therefore, is difficult to tune from a practical perspective. More recently, an alternative estimation algorithm called cubature Kalman filter (CKF) [12] has been proposed for general nonlinear systems. The CKF, which builds on the numerical-integration perspective of Gaussian filters [13, 14], employs a third-degree spherical-radical cubature rule to compute Gaussian-weighted integrals numerically and is a relatively new, derivative-free nonlinear filtering algorithm with improved performance over the UKF in terms of estimation accuracy, numerical stability and computational costs [12]. The performance of CKF, however, relies on the properly selected process and measurement noise statistics. For many applications, the model noise statistics are given before the filtering process and will maintain unchanged during the whole recursive process. However, this a priori statistical information is often unknown in practice, and even has time-varying characteristics. Adaptive filtering is trying to determine the statistical characteristics of nonlinear systems, and has recently gained attention. Maybeck [15] shows a maximum likelihood estimator to estimate system noise statistics. Busse et al. [16] modify Maybeck’s adaptation algorithm by properly choosing a window scale factor to control the level of the variance update. Lee and Alfriend [17] extend this adaptive scheme by introducing an innovative procedure to automatically calculate the window size using a derivative-free numerical optimization technique. The CKF has two implementation variants, standard CKF and square-root CKF (SR-CKF) [12, 18]. The SR-CKF is algebraically equivalent to the standard CKF and can consistently improve the numerical stability because all the resulting covariance matrices are guaranteed to stay positive semi-definite. In this paper, a new filtering formulation, which merges Lee and Alfriend’s adaptive process noise estimation [17] with the SR-CKF, is proposed for state estimation of general nonlinear systems. The resulting algorithm is thus referred to as square-root adaptive cubature Kalman filter (SR-ACKF). Furthermore, the process noise estimate for efficient square-
1406
root implementation is derived. As a simulation, the SR-ACKF is then applied to spacecraft attitude estimation from vector observations. The rest of this paper is organized as follows. The detailed derivation of square-root adaptive cubature Kalman filter is provided in Section II. Section III introduces the attitude kinematics and sensor models. Section IV describes the implementation details of the SR-ACKF algorithm for spacecraft attitude estimation. Section V presents the simulation results to compare the performance of the filters with each other. Finally, some concluding remarks are given in Section VI. II. S QUARE - ROOT ADAPTIVE CUBATURE K ALMAN FILTER In this section, we give the derivation of SR-ACKF for nonlinear state estimation. A. Process noise covariance estimation An attractive approach to the adaptive Kalman filtering problem is covariance matching, in which the process noise covariance Pv𝑘 and observation covariance Pn𝑘 themselves are adapted as measurements evolve with time. However, adaptive filtering algorithms that try to estimate both the system noise and the observation noise simultaneously are not robust. Since the measurement noise statistics are relatively well known compared to the system errors, we will only estimate process noise covariance Pv𝑘 adaptively in this work. Following Maybeck’s adaptation approach, the estimate for the process noise covariance is given by [15] −
ˆ P∗v𝑘 = Δx𝑘 Δx𝑇𝑘 + P− x𝑘 − Px𝑘 − Pv𝑘
(1)
ˆ𝑘 − ˆ where Δx𝑘 ≡ x x− 𝑘 is the state residual and represents the difference between the state estimate before the measurement update and after the measurement update. If this residual has a large value, then it indicates that the future state prediction is not accurate enough. P− x𝑘 and Px𝑘 denote a priori and a ˆ− posteriori state covariance estimate. P v𝑘 is the current expected process noise covariance. It is noteworthy that Maybeck’s estimator for system noise statistics is unbiased, i.e., the bias is assumed to equal to zero. Busse et al. extend Maybeck’s adaptation algorithm by combining the measure of the process noise P∗v𝑘 with the ˆ− current expected estimate P v𝑘 in a moving average [16] ) 1 ( ∗ ˆ− ˆ− ˆ v𝑘 = P Pv𝑘 − P P v𝑘 + v𝑘 𝐿𝑣
(2)
where 𝐿𝑣 is the window size that controls the level of expected update change. If 𝐿𝑣 is small, then each update is weighted heavily, but if 𝐿𝑣 is large, then each update has a small effect. The performance of the adaptive routine is very sensitive to the selection of 𝐿𝑣 , and thus should be properly selected for each application.
ˆ v𝑘 , in Eq. (2) can be The current process noise estimate, P rewritten for efficient square-root implementation as ( ) ) 𝐿𝑣 − 2 ˆ − 1 ( 1 ˆ Δx𝑘 Δx𝑇𝑘 + P− Pv𝑘 + Pv𝑘 = x𝑘 − Px𝑘 𝐿𝑣 𝐿𝑣 ( ) (3) ) − 𝐿 1 ( − 2 𝑣 𝑇 2 𝑇 ˆ Δx𝑘 Δx𝑘 + K𝑘 Py𝑘 K𝑘 Pv𝑘 + = 𝐿𝑣 𝐿𝑣 where 1 is a straightforward substitution of P∗v𝑘 given in Eq. (1), and 2 uses the Kalman state covariance update equation. B. Automated calculation of window scale factor The conventional approach to determine the window scale factor is an ad hoc process involving a considerable amount of trial-and-error to obtain near-optimal estimation results. Numerical optimization algorithms, however, can be used to replace the tedious, repetitive trial-and-error technique. The Downhill Simplex algorithm, which is a derivativefree numerical optimization technique, has been used to tune the parameters of the process noise covariance [19]. Lee and Alfriend use this method to automatically determine the window scale factor for nonlinear adaptive estimator [17]. This method will also be used in this paper. In order to apply the numerical optimization algorithm to determining the scale factor, a scalar objective function has to be constructed. A commonly used objective function in MonteCarlo performance simulation is the overall performance index 𝐽, which is defined as )12 ( 𝐿𝑚𝑐 1 ∑ 𝐽 2 (𝐿𝑣 ) (4) 𝐽 ≡ 𝐿𝑚𝑐 𝑖=1 𝑘,𝑖 where 𝐿𝑚𝑐 denotes the number of Monte-Carlo runs, 𝑖 represents the 𝑖th simulation run, and 𝐽𝑘,𝑖 (𝐿𝑣 ) is defined by )12 ( 𝐿𝑜𝑠 1 ∑ 𝐽𝑘,𝑖 (𝐿𝑣 ) ≡ ∥g𝑘,𝑖 ∥2 (5) 𝐿𝑜𝑠 𝑘=1
where 𝐿𝑜𝑠 is the length of entire observation sequences. The innovation g𝑘 = y𝑘 − ˆy− 𝑘 represents the difference between the actual and the predicted observations. C. Square-root adaptive cubature Kalman filter The general discrete-time nonlinear system model is given by x𝑘 = f(x𝑘−1 , v𝑘−1 , u1𝑘−1 ) y𝑘 = h(x𝑘 , n𝑘 , u2𝑘 )
(6a) (6b)
where x𝑘 ∈ ℝ𝑛𝑥 denotes the 𝑛𝑥 × 1 state vector and y𝑘 ∈ ℝ𝑛𝑦 the 𝑛𝑦 × 1 output observation vector. v𝑘 ∈ ℝ𝑛𝑣 is the 𝑛𝑣 × 1 state noise process vector and n𝑘 ∈ ℝ𝑛𝑛 the 𝑛𝑛 × 1 measurement noise vector. u1𝑘 ∈ ℝ𝑛𝑢1 is the 𝑛𝑢1 × 1 state input vector and u2𝑘 ∈ ℝ𝑛𝑢2 the 𝑛𝑢2 × 1 observation input vector. The mappings f : ℝ𝑛𝑥 × ℝ𝑛𝑣 × ℝ𝑛𝑢1 −→ ℝ𝑛𝑥 and h : ℝ𝑛𝑥 × ℝ𝑛𝑛 × ℝ𝑛𝑢2 −→ ℝ𝑛𝑦 represent the deterministic process and measurement models. It is assumed that v𝑘 and
1407
n𝑘 are uncorrelated zero-mean Gaussian noise processes with covariances given by Pv𝑘 and Pn𝑘 . The state random variable for SR-ACKF is redefined as the concatenation of the original state and the process and observation noise random variables [20]: ⎡ ⎤ ⎡ 𝑥⎤ x𝑘 x𝑘 (7) x𝑎𝑘 ≡ ⎣x𝑣𝑘 ⎦ ≡ ⎣v𝑘 ⎦ x𝑛𝑘 n𝑘
(b) Time update equations: ) ( 𝑥 − 𝑣 (𝑖), 𝒳𝑘−1 (𝑖), u1𝑘−1 𝒳𝑘𝑥 (𝑖) = f 𝒳𝑘−1 ˆx− 𝑘 =
𝑖=1
Π𝒳 𝑥− 𝑘
The effective dimension of this augmented state random variable is now 𝑛𝑎 = 𝑛𝑥 + 𝑛𝑣 + 𝑛𝑛 . In a similar manner the square root of augmented state covariance matrix is built up from the individual covariance of matrices of x, v and n [20]: ⎤ ⎡ 0 0 Sx𝑘 0 ⎦ (8) S𝑎𝑘 = ⎣ 0 Sv𝑘 0 0 S n𝑘
where the weights are given by [12] 1 𝑤𝑖𝑛𝑎 = , 𝑖 = 1, 2, . . . , 2𝑛𝑎 2𝑛𝑎 (c) Measurement update equations: ) ( − 𝑛 𝒴𝑘 (𝑖) = h 𝒳𝑘𝑥 (𝑖), 𝒳𝑘−1 (𝑖), u2𝑘
Π𝒴𝑘
𝑤𝑖𝑛𝑎 𝒴𝑘 (𝑖)
⎡ √ ( )𝑇 ⎤𝑇 𝑤1𝑛𝑎 𝒴𝑘 (1) − ˆy− 𝑘 ⎢ )𝑇 ⎥ ⎥ ⎢ √ 𝑛 ( − 𝑎 ⎥ ⎢ ˆ 𝒴 (2) − y 𝑤 𝑘 𝑘 2 ⎥ ⎢ = ⎢ ⎥ .. ⎥ ⎢ ⎥ ⎢ . ⎣√ ( )𝑇 ⎦ 𝑛𝑎 𝒴𝑘 (2𝑛𝑎 ) − ˆy− 𝑤2𝑛 𝑘 𝑎
Sy𝑘 = tria (Π𝒴𝑘 )
Px− y = Π𝒳 𝑥− Π𝑇𝒴𝑘 𝑘 𝑘 ) ( 𝑘 K𝑘 = Px𝑘 y𝑘 /S𝑇y𝑘 /Sy𝑘 ) ( g𝑘 = y𝑘 − h ˆx− 𝑘 , 0, u2𝑘 ˆx𝑘 =
where 𝐸[⋅] denotes expectation operator, Pv0 and Pn0 are the initial process and observation noise covariances. 2) for 𝑘 = 1, . . . , ∞,
Sx𝑘
(12b)
ˆx− 𝑘
+ K𝑘 g𝑘 ]) ([ = tria Π𝒳 𝑥− − K𝑘 Π𝒴𝑘 𝑘
(12c) (12d)
(13)
(14a) (14b)
(14c)
(14d) (14e) (14f) (14g) (14h) (14i)
(d) Process noise covariance estimation: ] [√ √ [ ] 𝐿𝑣 − 2 ˆ 1 Sv𝑘−1 , Πv𝑘 = K𝑘 g𝑘 , Sy𝑘 𝐿𝑣 𝐿𝑣
(a) Calculate the cubature points: 𝑖 = 1, 2, . . . , 2𝑛𝑎 (10)
(15a) ˆv = tria (Πv ) S 𝑘 𝑘 ⎤ ⎡ Sx𝑘 0 0 ˆv S𝑎𝑘 = ⎣ 0 S 0 ⎦ 𝑘 0 0 Sn𝑘
⎡
(11)
and ℰ 𝑛𝑎 is the standard cubature points set for a 𝑛𝑎 dimensional state space. The approach to calculate ℰ 𝑛𝑎 is presented in [12] and is not shown here for brevity.
2𝑛𝑎 ∑ 𝑖=1
]) ( [ ˆ x0 )(x0 − ˆx0 )𝑇 , x0 = 𝐸[x0 ] , Sx0 = chol 𝐸 (x0 − ˆ ˆv = chol (Pv ) , Sn = chol (Pn ) , S 0 0 0 0 ⎤ ⎡ 0 0 S x0 [ ] 𝑇 ˆv ˆ x𝑎0 = 𝐸[x𝑎0 ] = ˆ x𝑇0 , 0, 0 , S𝑎0 = ⎣ 0 S 0 ⎦ 0 0 0 Sn0 (9)
⎤ 𝑥 (𝑖) 𝒳𝑘−1 𝑎 𝑣 (𝑖)⎦ (𝑖) ≡ ⎣𝒳𝑘−1 𝒳𝑘−1 𝑛 𝒳𝑘−1 (𝑖)
)
𝑘
ˆy− 𝑘 =
(12a)
⎡ √ ( − )𝑇 ⎤𝑇 𝑤1𝑛𝑎 𝒳𝑘𝑥 (1) − ˆx− 𝑘 ⎢ )𝑇 ⎥ ⎥ ⎢ √ 𝑛 ( − − 𝑥 𝑎 ⎥ ⎢ 𝑤2 𝒳𝑘 (2) − ˆx𝑘 ⎥ ⎢ = ⎢ ⎥ .. ⎥ ⎢ ⎥ ⎢ . ⎣√ ( − )𝑇 ⎦ − 𝑛𝑎 𝑤2𝑛𝑎 𝒳𝑘𝑥 (2𝑛𝑎 ) − ˆ x𝑘
S− x𝑘 = tria Π𝒳 𝑥−
1) Initialization:
where
−
𝑤𝑖𝑛𝑎 𝒳𝑘𝑥 (𝑖)
(
where Sv and Sn are the square root of process and observation noise covariances, defined by SS𝑇 ≡ P. The SR-ACKF essentially propagates square-root factors of the predictive and posterior error covariances directly, thereby avoiding matrix square-rooting operations in each filtering cycle. Specifically speaking, the SR-ACKF uses the leastsquares method for the Kalman gain to avoid computing a matrix inversion explicitly, and matrix triangular factorizations or triangularizations (for instance, the QR decomposition) for covariance updates to compute a triangular square-root factor of the covariance. The entire algorithm is presented as follows:
𝑎 ˆ𝑎𝑘−1 +S𝑎𝑘−1 ℰ 𝑛𝑎 (𝑖), (𝑖) = x 𝒳𝑘−1
2𝑛𝑎 ∑
(15b) (15c)
It is noteworthy here that chol( ⋅ ) denotes a Cholesky decomposition of a matrix where a lower triangular Cholesky factor is returned, and tria( ⋅ ) is a general matrix triangularization algorithm which also generates a lower triangular
1408
matrix. The innovation g𝑘 is calculated using the actual measurement model and the propagated state, which incorporates the second-order terms of the measurement model into the state estimate update [21]. This is the most general form of the SR-ACKF. When the system and observation noises are assumed purely additive, the implementations of SR-ACKF need not augmented with the noise variables, thus reducing the number of cubature points and computational complexity. III. ATTITUDE KINEMATICS AND SENSOR MODELS In this section, we briefly review some preliminaries such as the quaternion attitude representation, gyro and attitude-vector models. The quaternion is a four-dimensional vector in which the scalar portion of the quaternion is the fourth element, defined as [ ] 𝝔 (16) q ≡ 𝑞4 with 𝝔 ≡ [𝑞1 , 𝑞2 , 𝑞3 ]
𝑇
= 𝒆 sin(𝜗/2)
𝑞4 = cos(𝜗/2)
(17a) (17b)
where 𝒆 is the unit Euler axis and 𝜗 is the rotation angle. Since a four-dimensional vector is used to describe three dimensions, the quaternion components cannot be independent of each other. The quaternion satisfies a normalization constraint given by q𝑇 q = ∥q∥2 = 1, which ensures that the DCM is orthonormal and is analogous to the fact that the Euler axis 𝒆 is a unit vector. The DCM (also called attitude matrix) is related to the quaternion by ) ( (18) 𝐴(q) = 𝑞42 − ∥𝝔∥2 𝐼3×3 + 2𝝔𝝔𝑇 − 2𝑞4 [𝝔×] where 𝐼3×3 is a 3 × 3 identity matrix and [𝝔×] is a crossproduct matrix since three-component vector cross-product operation 𝒂 × 𝒄 = [𝒂×]𝒄, which is defined by ⎤ ⎡ 0 −𝑞3 𝑞2 0 −𝑞1 ⎦ (19) [𝝔×] ≡ ⎣ 𝑞3 −𝑞2 𝑞1 0 An advantage of the quaternion is that successive rotations can be accomplished using quaternion multiplication, which can be written using [9] 𝐴(q)𝐴(q′ ) = 𝐴(q ⊗ q′ )
(20)
where the ⊗ vector operator denotes the quaternion multiplication, which represents the matrix operation q ⊗ q′ = [q⊗]q′ . The quaternion multiplication operator matrix [q⊗] is defined by ⎤ ⎡ 𝑞4 𝑞3 −𝑞2 𝑞1 ⎢−𝑞3 𝑞4 𝑞1 𝑞2 ⎥ ⎥ (21) [q⊗] ≡ ⎢ ⎣ 𝑞2 −𝑞1 𝑞4 𝑞3 ⎦ −𝑞1 −𝑞2 −𝑞3 𝑞4 Also, the inverse quaternion is defined by [ ] −𝝔 q−1 ≡ 𝑞4
(22)
Note that 𝐴(q−1 ) = 𝐴𝑇 (q) and q ⊗ q−1 = [0, 0, 0, 1]𝑇 , which is the identity quaternion. The quaternion kinematics equation is given by 1 (23) q˙ = Ω(𝝎)q 2 where 𝝎 = [𝜔1 , 𝜔2 , 𝜔3 ]𝑇 is the three-component angular rate vector and ] [ −[𝝎×] 𝝎 (24) Ω(𝝎) ≡ −𝝎 𝑇 0 A discrete-time approximation to the solution of Eq. (23) is given by [9] (25) q𝑘+1 = Λ(𝝎𝑘 )q𝑘 where
[ Λ(𝝎𝑘 ) ≡
𝑈𝑘
( 𝝍𝑘
]
)
(26a) −𝝍𝑘𝑇 cos ∥𝝎𝑘2∥Δ𝑡 ⎛ ( ∥𝝎 ∥Δ𝑡 ) ⎞ 𝑘 sin 2 ⎠𝝎 𝑘 𝝍𝑘 ≡ ⎝ (26b) ∥𝝎𝑘 ∥ ) ( ∥𝝎𝑘 ∥Δ𝑡 𝐼3×3 − [𝝍𝑘 ×] 𝑈𝑘 ≡ cos (26c) 2 and Δ𝑡 ≡ 𝑡𝑘+1 − 𝑡𝑘 is the sampling interval in the gyro output. Herein, we adopt a three-component generalized Rodrigues parameter (GRP) to represent the unconstrained attitude parameter, which is defined by [22] 𝜹𝝔 (27) 𝜹𝒑 ≡ 𝑙 ℎ + 𝛿𝑞4 where ℎ is a parameter from 0 to 1, 𝑙 is a scale factor and the error-quaternion 𝜹q is defined by 𝜹q ≡ q ⊗ q−1 𝑟
(28)
where q𝑟 denotes the reference quaternion which usually is ˆ and 𝜹q ≡ [𝜹𝝔𝑇 , 𝛿𝑞4 ]𝑇 . For small the estimated quaternion q angles the vector part of error-quaternion is approximately equal to half error angles [1]. We will choose 𝑙 = 2(ℎ + 1) so that ∥𝜹𝒑∥ is equal to 𝜗 for small errors. The inverse transformation from 𝜹𝒑 to 𝜹q is given by [9] √ −ℎ∥𝜹𝒑∥2 + 𝑙 𝑙2 + (1 − ℎ2 )∥𝜹𝒑∥2 (29a) 𝛿𝑞4 = 𝑙2 + ∥𝜹𝒑∥2 𝜹𝝔 = 𝑙−1 (ℎ + 𝛿𝑞4 )𝜹𝒑 (29b) A common sensor that measures the angular rate is a rateintegrating gyro. For this sensor, a widely used three-axis continuous-time model is given by [23] ˜ = 𝝎 + 𝜷 + 𝜼𝑣 𝝎 𝜷˙ = 𝜼𝑢
(30a) (30b)
˜ is the measured rate, 𝜷 is the gyro drift, and 𝜼𝑣 and where 𝝎 𝜼𝑢 are independent zero-mean Gaussian white-noise processes with ] [ (31a) 𝐸 𝜼𝑣 (𝑡)𝜼𝑣𝑇 (𝜏 ) = 𝜎𝑣2 𝛿(𝑡 − 𝜏 )𝐼3×3 [ ] 𝐸 𝜼𝑢 (𝑡)𝜼𝑢𝑇 (𝜏 ) = 𝜎𝑢2 𝛿(𝑡 − 𝜏 )𝐼3×3 (31b)
1409
where 𝛿(⋅) is the Dirac-delta function. The parameters of 𝜎𝑣 and 𝜎𝑢 can be experimentally obtained using frequency response data from the gyro outputs with units of rad/s1/2 and rad/s3/2 . For simulation purposes, discrete-time gyro measurements can be generated using the following recursive equations [24]: ( 2 )12 1 2 1 𝜎𝑣 ˜ 𝑘+1 = 𝝎𝑘+1 + (𝜷𝑘+1 + 𝜷𝑘 ) + + 𝜎 Δ𝑡 N𝑣 𝝎 2 Δ𝑡 12 𝑢 (32a) 1
𝜷𝑘+1 = 𝜷𝑘 + 𝜎𝑢 Δ𝑡 2 N𝑢
with
˜𝑖 denotes the 𝑖th 3 × 1 measurement vector, 𝒓𝑖 is the where 𝒃 𝑖th known 3 × 1 reference vector, and sensor error vector 𝝂𝑖 is Gaussian which satisfies 𝐸[𝝂𝑖 ] = 0 ] [ 𝐸 𝝂𝑖 𝝂𝑖𝑇 = 𝜎𝑖2 𝐼3×3 Multiple vector measurements can be given by ⎡˜ ⎤ ⎡ ⎡ ⎤ ⎤ 𝒃1 𝐴(q)𝒓1 𝝂1 ˜2 ⎥ ⎢𝒃 ⎢ 𝐴(q)𝒓2 ⎥ ⎢ 𝝂2 ⎥ ⎢ ⎥ ⎢ ⎢ ⎥ ⎥ y𝑘 = ⎢ . ⎥ = ⎢ ⎥ + ⎢ .. ⎥ .. ⎣ .. ⎦ ⎣ ⎣ . ⎦ ⎦ . ˜𝑁 𝐴(q)𝒓𝑁 𝝂𝑁 𝒃 𝑘
(34a) (34b)
𝑘
where 𝑁 is the number of measurement vectors provided by the attitude sensors. The measurement noise covariance in this case has a covariance of ]) ([ 2 (36) 𝐼3×3 Pn𝑘 = diag 𝜎12 𝐼3×3 , 𝜎22 𝐼3×3 , . . . , 𝜎𝑁 where diag( ⋅ ) denotes a diagonal matrix of appropriate dimension. IV. ATTITUDE ESTIMATION USING SQUARE - ROOT ADAPTIVE CUBATURE K ALMAN FILTER It is well known that when using a quaternion attitude representation, any updates produced from the Kalman type estimator can violate the quaternion unit-norm constraint. This can be alleviated by utilizing the quaternion representation for state propagation, but transforming this to an unconstrained attitude representation for the update.
[
𝑖 = 1, 2, . . . , 2𝑛𝑎
(38a) (38b)
where 𝜹 𝒑ˆ𝑘 denotes the estimated attitude error and 𝜷ˆ𝑘 represents the estimated gyro bias. 𝒳𝑘𝛿𝑝 denotes the attitude error part and 𝒳𝑘𝛽 represents gyro bias part. At each timestep, before an update is performed, the error-quaternion estimate, denoted by 𝜹ˆ q𝑘 (𝑖), can be transformed from the corresponding cubature points through Eq. (29). Then, the following quaternions are computed: ˆ𝑘, ˆ 𝑘 (𝑖) = 𝜹ˆ q𝑘 (𝑖) ⊗ q q
𝑖 = 1, 2, . . . , 2𝑛𝑎
(39)
The cubature-point quaternions in Eq. (39) are propagated using Eq. (25), with ( ) ˆ− ˆ 𝑘 (𝑖) q ˆ 𝑘 (𝑖), 𝑖 = 1, 2, . . . , 2𝑛𝑎 (40) q 𝑘+1 (𝑖) = Λ 𝝎 where the estimated angular rates are given by [9] ˜ 𝑘 − 𝒳𝑘𝛽 (𝑖), ˆ 𝑘 (𝑖) = 𝝎 𝝎
𝑖 = 1, 2, . . . , 2𝑛𝑎
(41)
The propagated error-quaternions are then computed using the definition of error-quaternion ( − )−1 ˆ− ˆ 𝑘+1 , 𝑖 = 1, 2, . . . , 2𝑛𝑎 (42) 𝜹ˆ q− 𝑘+1 (𝑖) = q 𝑘+1 (𝑖) ⊗ q with
(35)
] 𝒳𝑘𝛿𝑝 (𝑖) , 𝒳𝑘𝛽 (𝑖) ] [ 𝜹 𝒑ˆ ˆx𝑘 ≡ ˆ 𝑘 𝜷𝑘
𝒳𝑘𝑥 (𝑖) ≡
(32b)
where the subscript 𝑘 denotes the 𝑘th time-step, Δ𝑡 is the gyro sampling interval, and N𝑣 and N𝑢 are independent zero-mean Gaussian white-noise processes with covariance each given by the identity matrix. Typical vector observations on-board spacecrafts are the Earth magnetic field, the direction to the Sun, to the Moon, or to other celestial bodies. In general, the reference vectors, 𝒓, are known quite accurately from tables or almanacs, while the body vectors, 𝒃, are obtained from measurements corrupted by errors. As a result, the spacecraft attitude-vector measurement will be assumed to be in the form known as the QUEST measurement model [25], which for a single sensor is given by ˜𝑖 = 𝐴(q)𝒓𝑖 + 𝝂𝑖 (33) 𝒃
𝑘
We start the spacecraft attitude estimation by defining the following augmented cubature-point state vectors: ⎡ 𝑥 ⎤ 𝒳𝑘 (𝑖) (37) 𝒳𝑘𝑎 (𝑖) ≡ ⎣ 𝒳𝑘𝑣 (𝑖) ⎦ , 𝑖 = 1, 2, . . . , 2𝑛𝑎 𝒳𝑘𝑛 (𝑖)
) ( ˆ− ˆ𝑘 ˜ 𝑘 − 𝜷ˆ𝑘 q q 𝑘+1 = Λ 𝝎
(43)
The gyro bias is propagated using [9] − = 𝜷ˆ𝑘 𝜷ˆ𝑘+1
(44)
Therefore, the propagated cubature-point states, i.e., GRPs 𝛿𝑝 𝛽 (𝑖) and gyro biases 𝒳𝑘+1 (𝑖), are formed through Eqs. 𝒳𝑘+1 (27) and (44) with the square-root factor of estimated process noise covariance obtained from Eq. (15b). The predicted state mean and covariance are then computed using Eqs. (12b) and (12d). The propagated quaternions are used to calculate the cubature-point observations using Eq. (35), with ) ⎤ ⎡ ( − ˆ 𝑘+1 (𝑖))𝒓1 𝐴(q − ⎢𝐴 q ⎥ ⎢ ˆ 𝑘+1 (𝑖) 𝒓2 ⎥ 𝒴𝑘+1 (𝑖) = ⎢ ⎥ + 𝒳𝑘𝑛 (𝑖), 𝑖 = 1, 2, . . . , 2𝑛𝑎 .. ⎣ ⎦ ( − . ) ˆ 𝑘+1 (𝑖) 𝒓𝑁 𝐴 q (45) The observation mean and covariance are calculated using Eqs. (14b) and (14d) with measurement noise covariance given by Eq. (36). After the state estimate and covariance have been updated using Eqs. (14h) and (14i) from the most
1410
recent measurements, the actual attitude quaternion must be determined from the updated error-quaternion using
10
ˆ− ˆ 𝑘+1 = 𝜹ˆ q𝑘+1 ⊗ q q 𝑘+1
10
3
SR−CKF SR−ACKF
(46) RMSE−Attitude ( ° )
2
where 𝜹ˆ q𝑘+1 is transformed from the updated GRP, 𝜹 𝒑ˆ𝑘+1 , through Eq. (29). In this manner, the attitude quaternion estimation is performed without violation of the norm constraint. V. S IMULATION RESULTS In this section, a simulation with one vector measurement and three angular rate measurements is used to determine the attitude of a rotating spacecraft. The performance comparison of the SR-CKF and SR-ACKF is demonstrated through the simulation. For a fair comparison, a total of 100 independent Monte-Carlo runs are made and the root-mean suqare error (RMSE) of the attitude is chosen as the performance metric. For each run a different realization of both the process and observation noises is generated. The window scale factor for the SR-ACKF obtained from the Downhill Simplex algorithm is 86. Also, the corresponding parameters for the GRP are set to ℎ = 1 with 𝑙 = 4. All measurements are assumed to be sampled every 10 s and the total time of the simulation is 12 h. The attitude measurements are taken from a three-axis magnetometer (TAM) utilizing the vector measurement model previously discussed with a standard deviation of 50 nT. The gyro angular rate measurements are synthesized using Eq. (32) with gyro noise parameters 𝜎𝑣 = 3.1026 × 10−7 rad/s1/2 and 𝜎𝑢 = 9.0358 × 10−10 rad/s3/2 , and an initial bias of 0.6 ∘ /h for each axis. In order to stress the filters, the simulation run begins with the initial attitude errors of −60∘ , 70∘ and 20∘ for each axis, and the initial bias estimate of [0, 20, 0]𝑇 ∘ /h, respectively. The three attitude the initial covariance are each set to ( error parts)of 2 ∘ 50 , i.e., 50×(𝜋/180) rad2 . The three gyro(bias parts of the initial covariance are each set to 20 ∘ /h, i.e., 20 × (𝜋/(180 × )2 3600)) (rad/s)2 . The discrete process noise covariance for the SR-CKF is given by [9] ) ( 1 2 2) ] [( 2 1 2 3 𝜎𝑣 Δ𝑡 ( 1 +2 3 𝜎2𝑢)Δ𝑡 𝐼3×3 (2 𝜎2𝑢 Δ𝑡) 𝐼3×3 (47) Pv𝑘 = 𝐼3×3 𝜎𝑢 Δ𝑡 𝐼3×3 2 𝜎𝑢 Δ𝑡
1
10
0
10
−1
10
−2
10
0
2
4
Fig. 1.
6 Time (h)
8
10
12
RMSE in attitude
VI. C ONCLUSIONS In this paper a new approach for accurate and efficient state estimation of general nonlinear systems using the squareroot adaptive cubature Kalman filter is proposed. The ability to capture second order nonlinearities and the adaptive process noise covariance estimation of the proposed filter make it an attractive alternative to other nonlinear filters for spacecraft attitude estimation, which is inherently nonlinear. In order to avoid the quaternion normalization constraint, a three-component generalized Rodrigues parameter is used to represent the attitude for the update in the filer formulation. The performance of this novel filter in spacecraft attitude estimation is shown through the simulation.
A plot of the RMSE in attitude for the SR-CKF and SRACKF is shown in Fig. 1. As can be seen, the convergence performance of the SR-ACKF is far superior to that of the SRCKF. The SR-CKF converges slowly and takes almost 10 h to achieve a converged solution, while the SR-ACKF takes about 5 h. Furthermore, the errors of the SR-CKF are much higher than that of the SR-ACKF although they finally converge to similar value. The overall results obtained for the SR-CKF and SR-ACKF from the simulation clearly indicate that the SR-ACKF achieved the near-optimal performance in terms of the unknown uncertainties and neglected modeling errors. The rational behind this is that (1) the process noise statistics are adaptively tuned, and (2) the window scale factor is efficiently optimized by the Downhill Simplex algorithm.
1411
R EFERENCES [1] Shuster, M. D., “A Survey of Attitude Representations”, Journal of the Astronautical Sciences, Vol. 41, No. 4, October-December 1993, pp. 439-517. [2] Oshman, Y., and Markley, F. L., “Minimal-Parameter Attitude Matrix Estimation from Vector Observations”, Journal of Guidance, Control, and Dynamics, Vol. 21, No. 4, July-August 1998, pp. 595-602. [3] Farrell, J. L., “Attitude Determination by Kalman Filter”, Automatica, Vol. 6, No. 5, May 1970, pp. 419-430. [4] Pittelkau, M. E., “Rotation Vector in Attitude Estimation”, Journal of Guidance, Control, and Dynamics, Vol. 26, No. 6, November-December 2003, pp. 855-860. [5] Hamilton, W. R., Elements of Quaternions, Longmans, Green and Co., London, England, 1866. [6] Lefferts, E. J., Markley, F. L., and Shuster, M. D., “Kalman Filtering for Spacecraft Attitude Estimation”, Journal of Guidance, Control, and Dynamics, Vol. 5, No. 5, September-October 1982, pp. 417-429. [7] Psiaki, M. L., “Backward-Smoothing Extended Kalman Filter”, Journal of Guidance, Control, and Dynamics, Vol. 28, No. 5, September-October 2005, pp. 885-894.
[8] Zanetti, R., Majji, M., Bishop, R. H., and Mortari, D., “Norm-Constrained Kalman Filtering”, Journal of Guidance, Control, and Dynamics, Vol. 32, No. 5, SeptemberOctober 2009, pp. 1458-1465. [9] Crassidis, J. L., and Markley, F. L., “Unscented Filtering for Spacecraft Attitude Estimation”, Journal of Guidance, Control, and Dynamics, Vol. 26, No. 4, July-August 2003, pp. 536-542. [10] Julier, S. J., and Uhlmann, J. K., “Unscented Filtering and Nonlinear Estimation”, Proceedings of the IEEE, Vol. 92, No. 3, March 2004, pp. 401-422. [11] Julier, S. J., “The Scaled Unscented Transformation”, Proceedings of the American Control Conference, Anchorage, AK, May 2002, pp. 1108-1114. [12] Arasaratnam, I., and Haykin, S., “Cubature Kalman Filters”, IEEE Transactions on Automatic Control, Vol. 54, No. 6, June 2009, pp. 1254-1269. [13] Ito, K., and Xiong, K., “Gaussian Filters for Nonlinear Filtering Problems”, IEEE Transactions on Automatic Control, Vol. 45, No. 5, May 2000, pp. 910-927. [14] Wu, Y., Hu, D., Wu, M., and Hu, X., “A NumericalIntegration Perspective on Gaussian Filters”, IEEE Transactions on Signal Processing, Vol. 54, No. 8, August 2006, pp. 2910-2921. [15] Maybeck, P., Stochastic Models, Estimation, and Control, Vol. 2, Academic Press, New York, 1972. [16] Busse, F. D., How, J. P., and Simpson, J., “Demonstration of Adaptive Extended Kalman Filter for Low Earth Orbit Formation Estimation Using CDGPS”, Institute of Navigation GPS Meeting, Portland, OR, September 2002. [17] Lee, D.-J., and Alfriend, K. T., “Adaptive Sigma Point Filtering for State and Parameter Estimation”, AIAA/AAS Astrodynamics Specialist Conference and Exhibit, Providence, RI, August 2004, AIAA-2004-5101. [18] Arasaratnam, I., and Haykin, S., “Square-Root Quadrature Kalman Filtering”, IEEE Transactions on Signal Processing, Vol. 56, No. 6, June 2008, pp. 2589-2593. [19] Powell, T. D., “Automated Tuning of an Extended Kalman Filter Using the Downhill Simplex Algorithm”, Journal of Guidance, Control, and Dynamics, Vol. 25, No. 5, September-October 2002, pp. 901-908. [20] Wan, E. A., and van der Merwe, R., Kalman Filtering and Neural Networks, John Wiley & Sons, New York, NY, September 2001, Chap. 7. [21] Perea, L., and Elosegui, P., “New State Update Equation for the Unscented Kalman Filter”, Journal of Guidance, Control, and Dynamics, Vol. 31, No. 5, SeptemberOctober 2008, pp. 1500-1503. [22] Schaub, H., and Junkins, J. L., “Stereographic Orientation Parameters for Attitude Dynamics: A Generalization of the Rodrigues Parameters”, Journal of the Astronautical Sciences, Vol. 44, No. 1, January-March 1996, pp. 1-20. [23] Farrenkopf, R. L., “Analytic Steady-State Accuracy Solutions for Two Common Spacecraft Attitude Estimators”, Journal of Guidance and Control, Vol. 1, No. 4, July-
August 1978, pp. 282-284. [24] Crassidis, J. L, “Sigma-Point Kalman Filtering for Integrated GPS and Inertial Navigation”, AIAA Guidance, Navigation, and Control Conference, San Francisco, CA, August 2005, AIAA-2005-6052. [25] Shuster, M.D., and Oh, S. D., “Three-Axis Attitude Determination from Vector Observations”, Journal of Guidance and Control, Vol. 4, No. 1, January-February 1981, pp. 70-77.
1412
