Alternate Derivation of Geometric Extended Kalman Filter by MEKF Approach Lubin Chang Department of Navigation Engineering, Naval University of Engineering, China; changlubin@163.com I. Introduction In [1] a new attitude estimator referred as geometric extended Kalman filter (GEKF) is proposed, which is based on a new state vector error definition. In GEKF, the state vector error (gyroscope bias estimate error specifically for spacecraft attitude estimation) is put into a common frame using the estimated attitude error. In [1], the GEKF is derived for attitude estimation using covariance projection approach, which is an EKF that estimates the seven-component state vector (attitude quaternion + gyroscope bias). It is well known from [2, 3] that for quaternion Kalman filtering, there is another, yet more popularized approach, known as multiplicative EKF (MEKF) approach. It is pointed out in [3] that “the covariance projection idea is conceptually less satisfying than the MEKF”. So, it is desired to derive the geometric filtering within the MEKF framework. Actually, the time update of GEKF has been derived alternatively making use of MEKF approach in the Appendix of [1]. In this respect, this note is devoted to deriving the measurement update of the geometric filtering using the MEKF approach, resulting in the attitude estimator referred as geometric MEKF (GMEKF). The equivalence of the derived GMEKF and GEKF in [1] will also be demonstrated in this note. II. Alternate Derivation of the Geometric Filtering by MEKF Approach For brevity, this note uses the same set of symbols as in [1]. In MEKF for spacecraft attitude estimation, the Kalman filtering gain is given by 1 T T k k k k k k k − − −   = +   K P H H P H R (1) ( ) ( ) 1 3 3 3 3 ˆ ˆ k k k n − × − ×   Α ×   =     Α ×     q r 0 H q r 0   (2) The measurement update is given by [ ] [ ] 6 6 6 6 T T k k k k k k k k k + − × × = − − + P I K H P I K H K R K (3) ( ) ˆ ˆ k k k k k −   = −   Δx K y h x  (4) ˆ ˆ ˆ T T T k k k   =   Δx α db (5) ( ) ( ) ( ) 1 ˆ ˆ ˆ k k k k n − − −   Α   =     Α     q r h x q r  (6) The attitude quaternion is updated by ( ) ˆ ˆ ˆ ˆ 0.5 k k k k + − − = + q q Ξ q α (7) All the aforementioned equations are the same with those in traditional MEKF. What is fundamentally different in the derived GMEKF is the application of geometric gyroscope bias estimate error definition. According to [1], the geometric gyroscope bias defined in common coordinate frames is given by ( ) ˆ db = b - A dq b (8) For small attitude error, which is always the case for MEKF, the attitude matrix in (8) can be approximated as ( ) [ ] 3 3 × ≈ − × A α I α (9) In this respect, the gyroscope bias is updated by ˆ ˆ ˆ ˆ ˆ k k k k k   ≈ ×   + - - b b + b α + db (10) After that the attitude quaternion and gyroscope bias has been updated through (7) and (10), respectively, the corresponding Kalman filtering state estimate should be reset to zero, that is 3 1 ˆ k × = α 0 , 3 1 ˆ k × = db 0 (11) This reset procedure is used to “move information from one part of the estimate to another part” [4, 5]. After this reset procedure, there is an optional covariance modification as discussed in [3]. In order to make the presented GMEKF corresponding to GEKF in [1], the covariance modification is addressed as follows, which has learned many lessons from [3]. Before the presentation of the covariance modification, some vector definitions are firstly specified. All the quantities specified as follows are functions of time and, if not stated, their time dependences on are omitted for brevity. The pre-reset attitude error vector referenced to ˆ − q is given by ( ) + α . The reset attitude error vector referenced to ˆ + q is given by ( ) + + α . The pre-reset gyroscope error vector referenced to − b is given by ( ) + db . The reset gyroscope error vector referenced to + b is given by ( ) + + db . According to [3], the relationship between vectors related to attitude error is given by ( ) ( ) ( ) ( ) ( ) ( ) ˆ ˆ ∆ + + = + + − + + = + − + = ∆ +     α α α M α α M α (12) where ( ) ( ) ( ) ( ) ( ) { } 1 2 3 3 ˆ ˆ ˆ ˆ 1 4 2 T − + − × = = + + − +     M Ξ q Ξ q α I α (13) For the gyroscope bias, the true gyroscope bias does not care about the reference, so (8) gives ( ) ( ) ( ) ( ) ( ) ( ) ˆ = ˆ = − + + + + + + + + + b A α b db A α b db (14) According to (9), (14) can be approximated as ( ) { } ( ) ( ) { } ( ) 3 3 3 3 ˆ ˆ − × + × − + × + +     = − + + × + + +     I α b db I α b db (15) which is equivalent to ( ) ( ) ( ) ( ) ˆ ˆ ˆ ˆ − + − +     + + = + + − + × + − × + +     db b db b b α b α (16) According to (12) and remembering that ( ) 3 1 ˆ × + + = α 0 , (16) can be reorganized as ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) { } ( ) ( ) ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ ˆ − + − + − − + − + − +     + + = + + − + × + − × + − +               = + × + − + + + × + − + − × + − +                   = + − + + × − × + − +         db b db b b α b M α α b b α b db b α α b M α α db db b b M α α (17) From the second row to third row of (17) we have made used of the fact that ( ) ( ) ˆ ˆ ˆ ˆ ˆ + − −   = + × + + +   b b b α db (18) Denoting ( ) ( ) ( ) ( ) ( ) ( ) ˆ ˆ ∆ + = + − + ∆ + = + − + db db db α α α (19) and remembering that ( ) 3 1 ˆ × + + = db 0 by the definition of the reset, (17) can be rewritten as ( ) ( ) ( ) ( ) { } ( ) ˆ ˆ ˆ − + ∆ + + = + + − + +     = ∆ + + × − × ∆ +     db db db db b b M α (20) Given the transformation relationships (12) and (20), the covariance corresponding to the filtering state should be modified as follows after the reset procedure T ++ + = P MP M (21) where { } 3 3 3 3 ˆ ˆ × − + ×     =     × − ×         M 0 M b b M I (22) Eqs. (1)-(11) and (21) constitutes the measurement update of the GMEKF. III. Equivalence of GMEKF and GEKF In this section, the GMEKF measurement update will be approved to be equivalent to that of GEKF in [1]. According to [1], the Kalman gain of GEKF is given by 1 T T k k k k k k k − − −   = +   K P H H P H R (23) where k k k − = H H C  (24) ( ) ( ) ( ) ( ) 1 3 3 3 3 ˆ ˆ 2 ˆ ˆ 2 T k k k T k n k − − × − − ×     Α ×       =       Α ×     q r Ξ q 0 H q r Ξ q 0    (25) ( ) 4 3 3 3 ˆ 0.5 ˆ k k k − × − − ×     =     ×     Ξ q 0 C b I (26) Substituting (25) and (26) into (24) and making use of the fact that ( ) ( ) 3 3 ˆ ˆ T k k − − × = Ξ q Ξ q I give ( ) ( ) 1 3 3 3 3 ˆ ˆ k k k k n − × − ×   Α ×   = =     Α ×     q r 0 H H q r 0   (27) Therefore, k k = K K . In GEKF, the state is updated as ( ) ˆ ˆ ˆ ˆ ˆ k k k k k k k k k + − − − + −       = + −               q q C K y h x b b  (28) where ( ) ˆ k k − h x is the same with that in (6). Remembering that k k = K K and substituting (4), (5) and (26) into (28) give ( ) ˆ ˆ 0.5 ˆ ˆ ˆ ˆ ˆ ˆ k k k k k k k k k − + − + −         = +         ×             - Ξ q α q q b b b α + db (29) which is just the compact form of (7) and (10). From [1], it can be easily obtained that the updated state covariance of GEKF is the same with the modified covariance (21) given the fact that the updated attitude quaternion and gyroscope bias by GEKF is the same with that of GMEKF. Consequently, the two algorithms are mathematical equivalent. From the aforementioned derivation, it can be found that the attitude update of GEKF or GMEKF is the same with that of the traditional MEKF, given that the optional covariance modification step derived in [3] is added. The attitude update is a special step of these attitude estimators compared with traditional prediction-update Kalman filtering. In such structure, the propagated state vector in the filtering is virtually the non-constraint attitude error, while the global attitude, always in quaternion form, is used to perform attitude update. This is mainly used to maintain the quaternion normalization. In the GMEKF, the gyroscope bias is updated according to its geometric definition as shown in (8) and therefore, can also not be obtained directly by the traditional Kalman filtering prediction-update step. In this respect, in GMEKF the gyroscope bias error is firstly estimated as shown in (5) and then the global gyroscope bias is updated according to (10). Through lessons learned from [3], the covariance modification for gyroscope bias update can be derived and added. This is very similar with the attitude update procedure. IV. Conclusions In this note, the measurement update of geometric extended Kalman filter proposed in [1] is re-derived making use of the multiplicative extended Kalman filtering approach. In the derived geometric multiplicative filter, the geometric gyroscope bias error (global) is estimated by the Kalman filtering equation and used to update the gyroscope bias estimate (local). After the global gyroscope bias being updated, the local gyroscope bias error estimate is reset to zero for the next propagation. The covariance modification by this reset operation is also addressed in this note, which has learned many lessons from [3]. The attitude update part is the same with the traditional version. The presented algorithm derivation reveals an explicit relationship between the geometric extended Kalman filter and the multiplicative extended Kalman filter. References [1] M. S. Andrle and J. L. Crassidis, “Attitude Estimation Employing Common Frame Error Representations,” Journal of Guidance, Control, and Dynamics, vol. 38, no. 9, pp. 1614–1624, 2015. [2] E. J. Lefferts, F. L. Markley, and M. D. Shuster, “Kalman Filtering for Spacecraft Attitude Estimation,” Journal of Guidance, Control, and Dynamics, vol. 5, no. 5, pp. 417–429, 1982. [3] F. L. Markley, “Lessons Learned,” Journal of Astronautical Sciences, vol. 57, nos. 1–2, pp. 3–29, 2009. [4] F. L. Markley, “Attitude error representations for Kalman filtering,” Journal of Guidance, Control, and Dynamics, vol. 26, no. 2, pp. 311-317, 2003. [5] J. L. Crassidis, and F. L. Markley, “Unscented filtering for spacecraft attitude estimation,” Journal of Guidance, Control, and Dynamics, vol. 26, no. 4, pp. 536–542, 2003.