Hostname: page-component-cd9895bd7-dk4vv Total loading time: 0 Render date: 2024-12-23T08:52:51.881Z Has data issue: false hasContentIssue false
  • English
  • Français

Multiplicative extended Kalman filter ignoring initial conditions for attitude estimation using vector observations

Published online by Cambridge University Press:  12 January 2023

Lubin Chang
Lubin Chang*
Affiliation:
College of Electrical Engineering, Naval University of Engineering, Wuhan, Hubei, P.R. China
*
E-mail: [email protected]
Rights & Permissions [Opens in a new window]

Abstract

In this paper, the well-known multiplicative extended Kalman filter (MEKF) is re-investigated for attitude estimation using vector observations. From the Lie group theory, it is shown that the attitude estimation model is group-affine and its error state model should be trajectory-independent. Moreover, with such a trajectory-independent error state model, the linear Kalman filter is still effective for large initialisation errors. However, the measurement model of the traditional MEKF is dependent on the attitude prediction, which is therefore trajectory-dependent. This is also the main reason why the performance of traditional MEKF is degraded for large initialisation errors. Through substitution of the attitude prediction related term with vector observations in the body frame, a trajectory-independent measurement model is derived for MEKF. Meanwhile, MEKFs with reference attitude error definition and with global state formulating on special Euclidean group have also been studied, with the main focus on derivation of the trajectory-independent measurement models. Extensive Monte Carlo simulations of spacecraft attitude estimation implementations demonstrate that the performance of MEKFs can be much improved with trajectory-independent measurement models.

Keywords

inertial navigationlie groupmultiplicative extended Kalman filterattitude estimation
Type
Research Article
Information
The Journal of Navigation , Volume 76 , Issue 1 , January 2023 , pp. 62 - 76
Check for updates
Copyright
Copyright © The Author(s), 2023. Published by Cambridge University Press on behalf of The Royal Institute of Navigation

1. Introduction

Extended Kalman filter (EKF) has been widely used for attitude estimation using vector observations (Lefferts et al., Reference Lefferts, Markley and Shuster1982; Crassidis and Markley, Reference Crassidis and Markley2003; Markley, Reference Markley2003; Crassidis et al., Reference Crassidis, Markley and Cheng2007). With quaternion as the attitude parameterisation, the resultant EKF is known as multiplicative extended Kalman filter (MEKF) (Lefferts et al., Reference Lefferts, Markley and Shuster1982; Markley, Reference Markley2003). In MEKF, the quaternion is used for the global attitude propagation and a local unconstrained three-dimensional parameterisation is used for local attitude error estimation in filtering recursion. The MEKF has been widely approved as the workhorse of real-time attitude estimation using vector observations (Crassidis et al., Reference Crassidis, Markley and Cheng2007). However, the MEKF may become very difficult to tune and even prone to divergence when it has not been well initialised. In order to relax restrictions on valid initialisation region, some advanced filtering methods have been proposed (Crassidis and Markley, Reference Crassidis and Markley2003; Cheng and Crassidis, Reference Cheng and Crassidis2010). These advanced filtering methods can indeed outperform MEKF for large initialisation errors, but at the cost of increasing computational burden. In Andrle and Crassidis (Reference Andrle and Crassidis2015), a new frame-consistent EKF, termed geometric EKF (GEKF), is proposed for spacecraft attitude estimation. In GEKF, an attitude error related gyroscope bias error is defined based on common coordinates perspective. The GEKF can outperform MEKF for large initialisation errors with similar computational cost. In recent years, the invariant extended Kalman filter (IEKF) rooted in the matrix Lie group theory has been investigated for attitude or pose estimation in inertial based applications (Barrau and Bonnabel, Reference Barrau and Bonnabel2014, Reference Barrau and Bonnabel2015, Reference Barrau and Bonnabel2017; Barrau, Reference Barrau2015; Wang et al., Reference Wang, Wu, Zhou and He2018, Reference Wang, Wu, He, Li and Pan2019; Cui et al., Reference Cui, Wang, Wu and He2021). Barrau and Bonnabel (Reference Barrau and Bonnabel2014) argued that IEKF exhibits superior convergence and robustness properties over MEKF, due to its error state model not depending on the state estimate. Gui and de Ruiter (Reference Gui and de Ruiter2018) investigated two quaternion IEKFs for spacecraft attitude estimation based on the invariance theory and found that the MEKF can be viewed as a minor variant of the quaternion left IEKF (QLIEKF). According to Chang (Reference Chang2020), both the GEKF in Andrle and Crassidis (Reference Andrle and Crassidis2015) and the quaternion right IEKF (QRIEKF) in Gui and de Ruiter (Reference Gui and de Ruiter2018) can be derived by formulating the attitude and gyroscope drift bias together as elements of a special Euclidean group, $\mathrm{\mathbb{S}\mathbb{E}}(3 )$.

It is generally recognised that the performance degradation of MEKF with large initial attitude error owes much to local linearisation. In other words, the local linearised error state model is not applicable for cases with large attitude error. However, such a conclusion may not hold true universally. From the Lie group theory, if the group state dynamic model is group-affine, then the corresponding linear vector error state model will be trajectory-independent. Moreover, the nonlinear group error can be recovered exactly from the linear vector error (Barrau and Bonnabel, Reference Barrau and Bonnabel2017; Hartley et al., Reference Hartley, Ghaffari, Eustice and Grizzle2020). In other words, the linear approximation for group-affine will not lose accuracy. In this paper, it is demonstrated that the attitude kinematics model formulated on the special orthogonal group $\mathrm{\mathbb{S}{\mathbb O}}(3 )$ is group-affine and, therefore, the MEKF has the potential to handle large initialisation errors. Based on the aforementioned discussion, with a small modification of the measurement transition matrix of the existing MEKF, its performance can be much improved. Moreover, the MEKF with reference attitude error and MEKF with global state formulated on $\mathrm{\mathbb{S}\mathbb{E}}(3 )$ have also been revisited and investigated from the perspective of group-affine.

The remainder of this paper is organised as follows. Section 2 presents the dynamic model for attitude estimation using vector observations. The group-affine property of the model has also been analysed, which provides the theoretical basis for the probability of handling large initialisation errors with the linear state-space models. In Section 3, the state-space models with both body frame and reference frame attitude errors have been presented. A modified measurement model which is trajectory-independent is proposed for body frame attitude error definition. Meanwhile, through transformation of the innovation, the measurement model can also be transformed to trajectory-independent for reference frame attitude error definition. In Section 4, through formulating the attitude and gyroscope drift bias as a compact element of $\mathrm{\mathbb{S}\mathbb{E}}(3 )$, the corresponding vector error state-space models are presented. In Section 5, the explicit filtering procedures have been presented with different state-space models. The focus of this section is on the global state update procedures corresponding to their respective error definitions. Numerical simulations are given in Section 6 to evaluate the performance of these different state-space models involved in the MEKF, and conclusions are drawn in Section 7.

2. Group-affine analysis of attitude estimation model

2.1 System model

The attitude kinematics model in terms of attitude matrix is given by Crassidis and Junkins (Reference Crassidis and Junkins2012)

(1)\begin{equation}\dot{{\textbf{A}}}(t )={-} [{{\boldsymbol{\omega} }(t )\times } ]{\textbf{A}}(t )\end{equation}

where ${\boldsymbol{\omega} }$ is the angular rate measured by the gyroscopes and governed by

(2)\begin{align}\begin{aligned} \tilde{{\boldsymbol{\omega} }} & ={\boldsymbol{\omega} } + {\boldsymbol{\beta} } + {{\boldsymbol{\eta} }_{\textbf{v}}}\\ \dot{{\boldsymbol{\beta}}} & ={{\boldsymbol{\eta} }_{\textbf{u}}} \end{aligned}\end{align}

where ${\boldsymbol{\beta} }$ is the constant gyroscope bias. ${{\boldsymbol{\eta} }_{\textbf{v}}}$ and ${{\boldsymbol{\eta} }_{\textbf{u}}}$ are assumed to be zero-mean, Gaussian white-noise processes.

The measurement model corresponding to the vector observation is given by

(3)\begin{equation}{{\textbf{b}}_k} = {\textbf{A}}({{t_k}} ){{\textbf{r}}_k} + {{\boldsymbol{\upsilon} }_k}\end{equation}

where ${{\textbf{r}}_k}$ is the reference vector observation at time instant ${t_k}$ and ${{\textbf{b}}_k}$ is the observation vector with measurement noise ${{\boldsymbol{\upsilon} }_k}$. The measurement can be provided by sun sensors, horizon sensor, magnetometers and so on; for more details, refer to Markley and Crassidis (Reference Markley and Crassidis2014). Here only one vector observation is considered and the generalisation to case with multiple vector observations is straightforward.

2.2 Group-affine analysis of attitude estimation model

Given a group state model

(4)\begin{equation}\dot{{\boldsymbol{\chi} }} = {f_{\textbf{u}}}({\boldsymbol{\chi} } )\end{equation}

where ${\boldsymbol{\chi} } \in {\cal G}$ is the state on group ${\cal G}$. ${\textbf{u}}$ are the model inputs. From the Lie group theory, if the group state model in Equation (4) satisfies the following equation, then it is group-affine (Barrau and Bonnabel, Reference Barrau and Bonnabel2017; Hartley et al., Reference Hartley, Ghaffari, Eustice and Grizzle2020).

(5)\begin{equation}{f_{\textbf{u}}}({{{\boldsymbol{\chi} }_1}{{\boldsymbol{\chi} }_2}} )= {f_{\textbf{u}}}({{{\boldsymbol{\chi} }_1}} ){{\boldsymbol{\chi} }_2} + {{\boldsymbol{\chi} }_1}{f_{\textbf{u}}}({{{\boldsymbol{\chi} }_2}} )- {{\boldsymbol{\chi} }_1}{f_{\textbf{u}}}({{{\textbf{I}}_{nd}}} ){{\boldsymbol{\chi} }_2}\end{equation}

where ${{\boldsymbol{\chi} }_1},{{\boldsymbol{\chi} }_2} \in {\cal G}$.

It is known that attitude matrix ${\textbf{A}}(t )$ is on group $\mathrm{\mathbb{S}{\mathbb O}}(3 )$. Next, we will demonstrate that the attitude kinematics in Equation (1) is group-affine. In the following demonstration, the measurement errors of gyroscopes are not considered and the involved variables’ time dependences are omitted temporarily for brevity.

For the model in Equation (1), the group state is ${\textbf{A}}$ and the input is ${\boldsymbol{\omega} }$. According to Equation (1), we can get

(6a)\begin{gather}{f_{\textbf{u}}}({{{\textbf{A}}_1}{{\textbf{A}}_2}})={-} ({{\boldsymbol{\omega} } \times } ){{\textbf{A}}_1}{{\textbf{A}}_2}\end{gather}
(6b)\begin{gather}{f_{\textbf{u}}}({{{\textbf{A}}_1}} ){{\textbf{A}}_2} ={-} ({{\boldsymbol{\omega} } \times } ){{\textbf{A}}_1}{{\textbf{A}}_2}\end{gather}
(6c)\begin{gather}{{\textbf{A}}_1}{f_{\textbf{u}}}({{{\textbf{A}}_2}} ) ={-} {{\textbf{A}}_1}({{\boldsymbol{\omega} } \times } ){{\textbf{A}}_2}\end{gather}
(6d)\begin{gather}{{\textbf{A}}_1}{f_{\textbf{u}}}({{{\textbf{I}}_{3 \times 3}}} ){{\textbf{A}}_2} ={-} {{\textbf{A}}_1}({{\boldsymbol{\omega} } \times } ){{\textbf{A}}_2}\end{gather}

It can be easily checked that the attitude kinematics from Equation (1) satisfy Equation (5) and therefore, it is group-affine. According to Lie group theory (Barrau and Bonnabel, Reference Barrau and Bonnabel2017; Hartley et al., Reference Hartley, Ghaffari, Eustice and Grizzle2020), the vector error state models corresponding to the group-affine model are trajectory-independent. Moreover, if the observations take the following particular forms, then the linearised observation model will also be trajectory-independent.

(7)\begin{equation}\begin{array}{l} \textrm{Left-Invariant}\;\textrm{Observation: }{\textbf{y}} = {\boldsymbol{\chi} \mu }\\ \textrm{Rright-Invariant}\;\textrm{Observation: }{\textbf{y}} = {{\boldsymbol{\chi} }^{ - 1}}{\boldsymbol{\mu} } \end{array}\end{equation}

where ${\boldsymbol{\mu} }$ is a constant vector. It can be checked that the measurement model in Equation (3) is left-invariant and the linearised observation model corresponding to the left error definition will be trajectory-independent. However, at a certain time instant, both the observation vector ${{\textbf{b}}_k}$ and the reference vector ${{\textbf{r}}_k}$ can be viewed as known values. Therefore, Equation (3) can be rewritten as

(8)\begin{equation}{{\textbf{r}}_k} = {\textbf{A}}{({{t_k}} )^{ - 1}}{{\textbf{b}}_k} - {\textbf{A}}{({{t_k}} )^{ - 1}}{{\boldsymbol{\upsilon} }_k}\end{equation}

That is to say, the vector observation model can also be viewed as right-invariant. Therefore, the linearised observation model corresponding to the right error definition will also be trajectory-independent.

In the next section, we will present the explicit error state-space models for the attitude estimation problem using vector observations and their trajectory-independence will be clearly shown. This is also the starting point of the modification of the traditional MEKF.

3. Error state-space models on $\mathrm{\mathbb{S}{\mathbb O}}(3 )+ {\mathrm{\mathbb{R}}^3}$

3.1 Error state-space model with body frame attitude error

In the traditional MEKF, the attitude error is defined as

(9)\begin{equation}{\textbf{A}}({\boldsymbol{\delta} {\textbf{q}}}) = {\textbf{A}}({\textbf{q}}){\textbf{A}}{(\hat{{\textbf{q}}})^{ - 1}}\end{equation}

Here, we add the quaternion dependence of each attitude matrix because, in the MEKF framework, the global attitude is propagated in quaternion form. The attitude error in Equation (9) is in the right error form. From the frame transformation perspective, it denotes the attitude error in the body frame. This is a little different from those in Li and Mourikis (Reference Li and Mourikis2012), Barrau and Bonnabel (Reference Barrau and Bonnabel2014) and Gui and de Ruiter (Reference Gui and de Ruiter2018). The difference is caused by the fact that the global attitude matrix in Equations (1) and (3) denotes attitude transformation from reference frame to body frame while the attitude matrix in Li and Mourikis (Reference Li and Mourikis2012), Barrau and Bonnabel (Reference Barrau and Bonnabel2014) and Gui and de Ruiter (Reference Gui and de Ruiter2018) denotes attitude transformation from body frame to reference frame.

We term the error in Equation (9) as group error. The corresponding vector form is denoted as ${\boldsymbol{\delta} }{{\boldsymbol{\alpha} }^b}$. With the assumption of small attitude error, their relationship is given by

(10)\begin{equation}{\textbf{A}}({\boldsymbol{\delta} \textbf{q}}) = {{\textbf{I}}_{3 \times 3}} - ({{\boldsymbol{\delta} }{{\boldsymbol{\alpha} }^b} \times } )\end{equation}

We denote vector state as ${\boldsymbol{\delta} }{{\textbf{x}}_{\textrm{body}}} = {\begin{bmatrix} {{\boldsymbol{\delta} }{{\boldsymbol{\alpha} }^b}^T}& {{\boldsymbol{\delta} }{{\boldsymbol{\beta} }^T}} \end{bmatrix}^T}$, where ${\boldsymbol{\delta} \boldsymbol{\beta} } = {\boldsymbol{\beta} } - \hat{{\boldsymbol{\beta} }}$. The subscript ‘body’ means that the involved attitude error is expressed in the body frame. The process model corresponding to Equations (1) and (2) is given by

(11a)\begin{equation}{\boldsymbol{\delta} }{\dot{{\textbf{x}}}_{\textrm{body}}} = {{\textbf{F}}_{\textrm{body}}}({\hat{{\textbf{x}}}(t ),t} ){\boldsymbol{\delta} }{{\textbf{x}}_{\textrm{body}}} + {{\textbf{G}}_{\textrm{body}}}(t ){\textbf{w}}(t )\end{equation}

where

(11b)\begin{gather}{{\textbf{F}}_{\textrm{body}}}({\hat{{\textbf{x}}}(t ),t} )= \left[ {\begin{array}{*{20}{c}} { - ({\hat{{\boldsymbol{\omega} }} \times } )}& { - {{\textbf{I}}_{3 \times 3}}}\\ {{{\textbf{0}}_{3 \times 3}}}& {{{\textbf{0}}_{3 \times 3}}} \end{array}} \right]\end{gather}
(11c)\begin{gather}{{\textbf{G}}_{\textrm{body}}}(t )= \left[ {\begin{array}{*{20}{c}} { - {{\textbf{I}}_{3 \times 3}}}& {{{\textbf{0}}_{3 \times 3}}}\\ {{{\textbf{0}}_{3 \times 3}}}& {{{\textbf{I}}_{3 \times 3}}} \end{array}} \right]\end{gather}

In Equation (11a,b), ${\textbf{w}}(t )= {\left[ {\begin{array}{*{20}{c}} {{\boldsymbol{\eta} }_{\textbf{v}}^T(t )}& {{\boldsymbol{\eta} }_{\textbf{u}}^T(t )} \end{array}} \right]^T}$, $\hat{{\boldsymbol{\omega} }} = \tilde{{\boldsymbol{\omega} }} - \hat{{\boldsymbol{\beta} }}$. It can be seen from Equation (11) that if the gyroscope bias is not considered, the differential model corresponding to ${\boldsymbol{\delta} }{{\boldsymbol{\alpha} }^b}$ is independent of ${\textbf{A}}(\hat{{\textbf{q}}})$. This is consistent with the group-affine analysis of Equation (1). Unfortunately, as noted in Barrau and Bonnabel (Reference Barrau and Bonnabel2017) and Hartley et al. (Reference Hartley, Ghaffari, Eustice and Grizzle2020), there is no Lie group that includes the bias terms while also having dynamics that satisfy the group-affine property. However, much of the group-affine structure of attitude can be preserved even with bias augmentation.

With the attitude error definition in Equation (9), the linearised measurement transition matrix corresponding to Equation (3) is given by Crassidis and Junkins (Reference Crassidis and Junkins2012)

(12)\begin{equation}{{\textbf{H}}_{\textrm{body,}}}_k({\hat{{\textbf{x}}}_k^ - } )= \left[ {\begin{array}{@{}cc@{}} {[{{\textbf{A}}(\hat{{\textbf{q}}}_k^ - ){{\textbf{r}}_k} \times } ]}& {{{\textbf{0}}_{3 \times 3}}} \end{array}} \right]\end{equation}

It appears that Equation (12) is dependent on the global attitude prediction ${\textbf{A}}(\hat{{\textbf{q}}}_k^ - )$, which is not consistent with the invariance analysis of the vector observation model. Essentially, this is just the reason why the traditional MEKF does not perform so well with large initialisation errors in previous works. However, according to Equations (3) and (12), a modified measurement transition matrix can be given by

(13)\begin{equation}{{\textbf{H}}_{\textrm{body,}}}_k = \left[ {\begin{array}{@{}cc@{}} {({{{\tilde{{\textbf{b}}}}_k} \times } )}& {{{\textbf{0}}_{3 \times 3}}} \end{array}} \right]\end{equation}

where ${\tilde{{\textbf{b}}}_k}$ denotes the measured vector observation with noise. It is now shown that the measurement transition matrix in Equation (13) is trajectory-independent. According to the Lie group theory, it is possible to recover the nonlinear attitude error ${\textbf{A}}({\boldsymbol{\delta} q})$ with the linear state-space model in Equations (11) and (13). Here we have not claimed that the nonlinear attitude error can be recovered ‘exactly’ from its linear vector error state estimate. This is because the gyroscope bias augmentation has made the process model not totally trajectory-independent, as shown in Equation (11).

3.2 Error state-space model with reference frame attitude error

For MEKF, the attitude error can also be defined as

(14)\begin{equation}{\textbf{A}}({{\boldsymbol{\delta} \textbf{q}}} )= {\textbf{A}}{({\hat{{\textbf{q}}}} )^{ - 1}}{\textbf{A}}({\textbf{q}} )\end{equation}

The attitude error in Equation (14) is in the left error form. From the frame transformation perspective, it denotes the attitude error in the reference (inertial) frame. The MEKF with reference attitude error definition was first studied by Gai et al. (Reference Gai, Daly, Harrison and Lemos1985). The attitude quaternion in Gai et al. (Reference Gai, Daly, Harrison and Lemos1985) defines the transformation from the body frame to the inertial frame, which is different from that in Lefferts et al. (Reference Lefferts, Markley and Shuster1982), Markley (Reference Markley2003); Crassidis and Junkins (Reference Crassidis and Junkins2012), and Andrle and Crassidis (Reference Andrle and Crassidis2015). With the same attitude definition as in those works, and Crassidis and Junkins (Reference Crassidis and Junkins2012), the state-space model for MEKF with reference attitude error definition is re-derived in Chang (Reference Chang2020).

Similarly to Equation (10), the attitude error in Equation (14) can also be approximated as

(15)\begin{equation}{\textbf{A}}({\boldsymbol{\delta} \textbf{q}}) = {{\textbf{I}}_{3 \times 3}} - ({{\boldsymbol{\delta} }{{\boldsymbol{\alpha} }^r} \times } )\end{equation}

We denote the vector state as ${\boldsymbol{\delta} }{{\textbf{x}}_{\textrm{ref}}} = {\left[ {\begin{array}{@{}cc@{}} {{\boldsymbol{\delta} }{{\boldsymbol{\alpha} }^r}^T}& {{\boldsymbol{\delta} }{{\boldsymbol{\beta} }^T}} \end{array}} \right]^T}$, where ${\boldsymbol{\delta} \beta } = {\boldsymbol{\beta} } - \hat{{\boldsymbol{\beta} }}$. The subscript ‘ref’ means that the involved attitude error is expressed in the reference frame. The process model of ${\boldsymbol{\delta} }{{\textbf{x}}_{\textrm{ref}}}$ is given by

(16a)\begin{equation}{\boldsymbol{\delta} }{\dot{{\textbf{x}}}_{\textrm{ref}}} = {{\textbf{F}}_{\textrm{ref}}}({\hat{{\textbf{x}}}(t ),t} ){\boldsymbol{\delta} }{{\textbf{x}}_{\textrm{ref}}} + {{\textbf{G}}_{\textrm{ref}}}(t ){\textbf{w}}(t )\end{equation}

where

(16b)\begin{gather}{{\textbf{F}}_{\textrm{ref}}}({\hat{{\textbf{x}}}(t ),t} )= \left[ {\begin{array}{@{}cc@{}} {{{\textbf{0}}_{3 \times 3}}}& { - {\textbf{A}}{{({\hat{{\textbf{q}}}} )}^T}}\\ {{{\textbf{0}}_{3 \times 3}}}& {{{\textbf{0}}_{3 \times 3}}} \end{array}} \right]\end{gather}
(16c)\begin{gather}{{\textbf{G}}_{\textrm{ref}}}(t )= \left[ {\begin{array}{@{}cc@{}} { - {\textbf{A}}{{({\hat{{\textbf{q}}}} )}^T}}& {{{\textbf{0}}_{3 \times 3}}}\\ {{{\textbf{0}}_{3 \times 3}}}& {{{\textbf{I}}_{3 \times 3}}} \end{array}} \right]\end{gather}

It can be seen from Equation (16) that, if the gyroscope bias is not considered, the differential model corresponding to ${\boldsymbol{\delta} }{{\boldsymbol{\alpha} }^r}$ is independent of ${\textbf{A}}(\hat{{\textbf{q}}})$. This is consistent with the group-affine analysis of Equation (1). The augmentation of gyroscope bias introduces model dependence of the global attitude estimate ${\textbf{A}}(\hat{{\textbf{q}}})$.

With the attitude error definition in Equation (15), the linearised measurement model corresponding to Equation (3) is given by Chang (Reference Chang2020)

(17)\begin{equation} {{\textbf{H}}_{\textrm{ref,}}}_k({\hat{{\textbf{x}}}_k^ - } )= \left[ {\begin{array}{@{}cc@{}} {[{({{\textbf{A}}(\hat{{\textbf{q}}}_k^ - ){{\textbf{r}}_k}} )\times } ]{\textbf{A}}({\hat{{\textbf{q}}}_k^ - } )}& {{{\textbf{0}}_{3 \times 3}}} \end{array}} \right]\end{equation}

It is shown that the measurement transition matrix in Equation (17) is dependent on the global attitude prediction ${\textbf{A}}(\hat{{\textbf{q}}}_k^ - )$, which is not consistent with the invariance analysis of the vector observation model. However, Equation (17) can be reorganised as

(18)\begin{align} {{\textbf{H}}_{\textrm{ref,}}}_k({\hat{{\textbf{x}}}_k^ - } )& =\left[ {\begin{array}{@{}cc@{}} {{\textbf{A}}({\hat{{\textbf{q}}}_k^-} ){\textbf{A}}{{({\hat{{\textbf{q}}}_k^ - } )}^{ - 1}}[{({{\textbf{A}}(\hat{{\textbf{q}}}_k^ - ){{\textbf{r}}_k}} )\times } ]{\textbf{A}}({\hat{{\textbf{q}}}_k^ - } )}& {{{\textbf{0}}_{3 \times 3}}} \end{array}} \right]\nonumber\\ & =\left[ {\begin{array}{@{}cc@{}} {{\textbf{A}}({\hat{{\textbf{q}}}_k^ - } )[{({{\textbf{A}}{{({\hat{{\textbf{q}}}_k^ - } )}^{ - 1}}{\textbf{A}}(\hat{{\textbf{q}}}_k^ - ){{\textbf{r}}_k}} )\times } ]}& {{{\textbf{0}}_{3 \times 3}}} \end{array}} \right]\\ & =\left[ {\begin{array}{@{}cc@{}} {{\textbf{A}}({\hat{{\textbf{q}}}_k^ - } )({{{\textbf{r}}_k} \times } )}& {{{\textbf{0}}_{3 \times 3}}} \end{array}} \right] \nonumber\end{align}

Although Equation (18) is now still dependent on the global attitude, through operation in the following theorem, it is possible to derive a global attitude independent measurement transition matrix.

Theorem 1 (Barrau, Reference Barrau2015): Applying the same linear function to the linearised measurement transition matrix and innovation term of an EKF before computing the gains does not change the results of the filter.

The explicit proof is presented in Barrau (Reference Barrau2015). Based on Theorem 1, the matrix in Equation (18) can be transformed as

(19)\begin{equation}{{\textbf{H}}_{\textrm{ref}}}_{,k,\textrm{trans}} = {\textbf{A}}{({\hat{{\textbf{q}}}_k^ - } )^{ - 1}}{{\textbf{H}}_k}({\hat{{\textbf{x}}}_k^ - } )= \left[ {\begin{array}{@{}cc@{}} {({{{\textbf{r}}_k} \times } )}& {{{\textbf{0}}_{3 \times 3}}} \end{array}} \right]\end{equation}

Accordingly, the innovation should be transformed as

(20)\begin{equation}{{\textbf{n}}_{k,\textrm{trans}}} = {\textbf{A}}{({\hat{{\textbf{q}}}_k^ - } )^{ - 1}}[{{{\textbf{b}}_k} - {\textbf{A}}({\hat{{\textbf{q}}}_k^ - } ){{\textbf{r}}_k}} ]= {\textbf{A}}{({\hat{{\textbf{q}}}_k^ - } )^{ - 1}}{{\textbf{b}}_k} - {{\textbf{r}}_k}\end{equation}

The measurement noise covariance should also be transformed as

(21)\begin{equation}{{\textbf{R}}_{k,\textrm{trans}}} = {\textbf{A}}{({\hat{{\textbf{q}}}_k^ - } )^{ - 1}}{{\textbf{R}}_k}{\textbf{A}}({\hat{{\textbf{q}}}_k^ - } )\end{equation}

where ${{\textbf{R}}_k}$ is the covariance corresponding to ${{\boldsymbol{\upsilon} }_k}$ in Equation (3).

Now, the measurement transition matrix ${{\textbf{H}}_{\textrm{ref}}}_{,k,\textrm{trans}}$ is trajectory-independent. It should be noted that making use of the trajectory-independent matrix of Equation (19) and the transformed innovation of Equation (20) offers the same performance as that making use of Equation (17) or (18) and traditional innovation. The transformation procedures from Equations (18) to (21) are only used to demonstrate that the invariant measurement of Equation (3) can result in a trajectory-independent measurement transition matrix corresponding to reference attitude error definition.

Remark 1: It is known from Section 2 that the attitude kinematics from Equation (1) is group-affine and therefore its error state model is trajectory-independent. It can be seen from Equations (11) and (16) that the attitude error state model is indeed trajectory-independent. Moreover, these trajectory-independent attitude error state models will also be valid when the initial attitude error is large. However, this statement is seemingly contradictory with the approximation in Equations (10) and (15) when deriving the attitude error state model. In the Appendix, we have derived the attitude error state model without any approximation. This can further support that the linear model can be used to accomplish the attitude estimation with large initial attitude error, as will be shown in Section 6.

4. Error state-space models on $\mathrm{\mathbb{S}\mathbb{E}}(3 )$

In Chang (Reference Chang2020), the attitude estimation has also been investigated making use of $\mathrm{\mathbb{S}\mathbb{E}}(3 )$. Through formulating the attitude and gyroscope bias as a compact state on $\mathrm{\mathbb{S}\mathbb{E}}(3 )$, both the GEKF and QRIEKF can be derived. In this section, the explicit error state-space model is directly presented with group state definition on $\mathrm{\mathbb{S}\mathbb{E}}(3 )$.

For attitude estimation, the group state on $\mathrm{\mathbb{S}\mathbb{E}}(3 )$ is defined as

(22)\begin{equation}{\boldsymbol{\chi} } = \left[ {\begin{array}{@{}cc@{}} {{\textbf{A}}({\textbf{q}})}& {\boldsymbol{\beta} }\\ {{{\textbf{0}}_{1 \times 3}}}& 1 \end{array}} \right]\end{equation}

It can also be demonstrated that with the group state from Equation (22), its dynamic model does not satisfy Equation (5). Therefore, the resulting error state model will be dependent on the global state estimate, which will be shown in Equations (25) and (28).

4.1 Error state-space model with right error

The right group error corresponds to Equation (9) and it is defined as

(23)\begin{equation}{\boldsymbol{\delta} }{{\boldsymbol{\chi} }_{\textrm{right}}} = {\boldsymbol{\chi} }{\hat{{\boldsymbol{\chi} }}^{ - 1}} = \left[ {\begin{array}{@{}cc@{}} {{\textbf{A}}({\textbf{q}}){\textbf{A}}{{(\hat{{\textbf{q}}})}^{ - 1}}}& {{\boldsymbol{\beta} } - {\textbf{A}}({\textbf{q}}){\textbf{A}}{{(\hat{{\textbf{q}}})}^{ - 1}}\hat{{\boldsymbol{\beta} }}}\\ {{{\textbf{0}}_{1 \times 3}}}& 1 \end{array}} \right]\end{equation}

The vector form corresponding to ${\boldsymbol{\delta} }{{\boldsymbol{\chi} }_{\textrm{right}}}$ is denoted as ${\textbf{d}}{{\textbf{x}}_{\textrm{right}}} = {\left[ {\begin{array}{@{}cc@{}} {{\textbf{d}}{{\boldsymbol{\alpha} }^b}^T}& {{\textbf{d}}{{\boldsymbol{\beta} }^b}^T} \end{array}} \right]^T}$. The subscript ‘right’ means that the error state is corresponding to the right group error. ${\textbf{d}}{{\boldsymbol{\alpha} }^b}$ is corresponding to the attitude error ${\textbf{A}}({\textbf{q}}){\textbf{A}}{(\hat{{\textbf{q}}})^{ - 1}}$. It is clearly shown that ${\textbf{d}}{{\boldsymbol{\alpha} }^b}$ is just the attitude error in Equation (9). ${\textbf{d}}{{\boldsymbol{\beta} }^b}$ is corresponding to the gyroscope drift bias error and is given by

(24)\begin{align} {\textbf{d}}{{\boldsymbol{\beta} }^b} &= {\boldsymbol{\beta} } - {\textbf{A}}({\textbf{q}}){\textbf{A}}{(\hat{{\textbf{q}}})^{ - 1}}\hat{{\boldsymbol{\beta} }}\nonumber\\ & = {\boldsymbol{\beta} } - [{{{\textbf{I}}_{3 \times 3}} - ({{\textbf{d}\boldsymbol{\alpha} } \times } )} ]\hat{{\boldsymbol{\beta} }} = {\boldsymbol{\delta} \beta } - ({\hat{{\boldsymbol{\beta} }} \times } ){\textbf{d}\boldsymbol{\alpha} } \end{align}

where we have made the same approximation for ${\textbf{d}}{{\boldsymbol{\alpha} }^b}$ as Equation (10).

The process model of ${\textbf{d}}{{\textbf{x}}_{\textrm{right}}}$ is given by

(25a)\begin{equation}{\textbf{d}}{\dot{{\textbf{x}}}_{\textrm{right}}} = {{\textbf{F}}_{\textrm{right}}}({\hat{{\boldsymbol{\chi} }}(t),t} ){\textbf{d}}{{\textbf{x}}_{\textrm{right}}} + {{\textbf{G}}_{\textrm{right}}}({\hat{{\boldsymbol{\chi} }}(t),t} ){\textbf{w}}(t )\end{equation}

where

(25b)\begin{gather}{{\textbf{F}}_{\textrm{right}}}({\hat{{\boldsymbol{\chi} }}(t),t} )= \left[ {\begin{array}{@{}cc@{}} { - ({\tilde{{\boldsymbol{\omega} }} \times } )}& { - {{\textbf{I}}_{3 \times 3}}}\\ {({\hat{{\boldsymbol{\beta} }} \times } )({\hat{{\boldsymbol{\omega} }} \times } )}& {({\hat{{\boldsymbol{\beta} }} \times } )} \end{array}} \right]\end{gather}
(25c)\begin{gather}{{\textbf{G}}_{\textrm{right}}}({\hat{{\boldsymbol{\chi} }}(t),t} )= \left[ {\begin{array}{@{}cc@{}} { - {{\textbf{I}}_{3 \times 3}}}& {{{\textbf{0}}_{3 \times 3}}}\\ {({\hat{{\boldsymbol{\beta} }} \times } )}& {{{\textbf{I}}_{3 \times 3}}} \end{array}} \right]\end{gather}

It is clearly shown that Equation (25b) is just the process transition matrix used in GEKF. The defined error state ${\textbf{d}}{{\textbf{x}}_{\textrm{right}}}$ is a little different from that in Chang (Reference Chang2020). This is because we intend to derive a consistent result with GEKF. It is shown in Equation (25) that, with the right error definition, the process transition matrix is now dependent on the global state estimate $\hat{{\boldsymbol{\beta} }}$. This is also the result of the fact that the dynamic model of the group state in Equation (22) is not group-affine.

Since the attitude error element of ${\textbf{d}}{{\textbf{x}}_{\textrm{right}}}$ has the same definition as that of ${\boldsymbol{\delta} }{{\textbf{x}}_{\textrm{body}}}$, the measurement transition matrix corresponding to ${\textbf{d}}{{\textbf{x}}_{\textrm{right}}}$ is the same Equation (12) or (13). Based on the invariance theory, Equation (13) will be preferable to Equation (12).

4.2 Error state-space model with left error

The left group error corresponds to Equation (14) and is defined as

(26)\begin{equation}{\boldsymbol{\delta} }{{\boldsymbol{\chi} }_{\textrm{left}}} = {\hat{{\boldsymbol{\chi} }}^{ - 1}}{\boldsymbol{\chi} } = \left[ {\begin{array}{@{}cc@{}} {{\textbf{A}}{{(\hat{{\textbf{q}}})}^{ - 1}}{\textbf{A}}({\textbf{q}})}& {{\textbf{A}}{{(\hat{{\textbf{q}}})}^{ - 1}}({{\boldsymbol{\beta} } - \hat{{\boldsymbol{\beta} }}} )}\\ {{{\textbf{0}}_{1,3}}}& 1 \end{array}} \right]\end{equation}

The vector form corresponding to ${\boldsymbol{\delta} }{{\boldsymbol{\chi} }_{\textrm{left}}}$ is denoted as ${\textbf{d}}{{\textbf{x}}_{\textrm{left}}} = {\left[ {\begin{array}{@{}cc@{}} {{\textbf{d}}{{\boldsymbol{\alpha} }^r}^T}& {{\textbf{d}}{{\boldsymbol{\beta} }^r}^T} \end{array}} \right]^T}$. The subscript ‘left’ means that the error state is corresponding to the left group error. ${\textbf{d}}{{\boldsymbol{\alpha} }^r}$ is corresponding to the attitude error ${\textbf{A}}{(\hat{{\textbf{q}}})^{ - 1}}{\textbf{A}}({\textbf{q}})$. It is clearly shown that ${\textbf{d}}{{\boldsymbol{\alpha} }^r}$ is just the attitude error in Equation (14). ${\textbf{d}}{{\boldsymbol{\beta} }^r}$ is corresponding to the gyroscope bias error and is given by

(27)\begin{equation}{\textbf{d}}{{\boldsymbol{\beta} }^r} = {\textbf{A}}{(\hat{{\textbf{q}}})^{ - 1}}({{\boldsymbol{\beta} } - \hat{{\boldsymbol{\beta} }}} )= {\textbf{A}}{(\hat{{\textbf{q}}})^{ - 1}}{\boldsymbol{\delta} \beta }\end{equation}

From the attitude transformation perspective, Equation (27) can be viewed as transforming the gyroscope bias error from body frame to reference frame. This is also the insight in Gui and de Ruiter (Reference Gui and de Ruiter2018), that ‘the group actions can be intuitively viewed as some coordinate transformations’.

The process model of ${\textbf{d}}{{\textbf{x}}_{\textrm{left}}}$ is given by

(28a)\begin{equation}{\textbf{d}}{\dot{{\textbf{x}}}_{\textrm{left}}} = {{\textbf{F}}_{\textrm{left}}}({\hat{{\boldsymbol{\chi} }}(t),t} ){\textbf{d}}{{\textbf{x}}_{\textrm{left}}} + {{\textbf{G}}_{\textrm{left}}}({\hat{{\boldsymbol{\chi} }}(t),t} ){\textbf{w}}(t )\end{equation}

where

(28b)\begin{gather}{{\textbf{F}}_{\textrm{left}}}({\hat{{\boldsymbol{\chi} }}(t),t} )= \left[ {\begin{array}{@{}cc@{}} {{{\textbf{0}}_{3 \times 3}}}& { - {{\textbf{I}}_{3 \times 3}}}\\ {{{\textbf{0}}_{3 \times 3}}}& {[{({{\textbf{A}}{{({\hat{{\textbf{q}}}} )}^T}\hat{{\boldsymbol{\omega} }}} )\times } ]} \end{array}} \right]\end{gather}
(28c)\begin{gather}{{\textbf{G}}_{\textrm{left}}}({\hat{{\boldsymbol{\chi} }}(t),t} )= \left[ {\begin{array}{@{}cc@{}} { - {\textbf{A}}{{({\hat{{\textbf{q}}}} )}^T}}& {{{\textbf{0}}_{3 \times 3}}}\\ {{{\textbf{0}}_{3 \times 3}}}& {{\textbf{A}}{{({\hat{{\textbf{q}}}} )}^T}} \end{array}} \right]\end{gather}

It is clearly shown that Equation (28b) is just the process transition matrix used in QRIEKF in Gui and de Ruiter (Reference Gui and de Ruiter2018). The defined error state ${\textbf{d}}{{\textbf{x}}_{\textrm{left}}}$ is a little different from that in Chang (Reference Chang2020). This is because we intend to derive the consistent result with QRIEKF. In this paper, the process model with the form as in Equation (28) is corresponding to left group state error while such form is corresponding to the right group state error in Gui and de Ruiter (Reference Gui and de Ruiter2018). This is also caused by the difference between the meanings of the attitude matrix. Specifically, the attitude matrix in this paper denotes the attitude transformation from reference frame to body frame, while the attitude matrix in Gui and de Ruiter (Reference Gui and de Ruiter2018) denotes attitude transformation from body frame to reference frame. It is shown that Equation (28b) is also dependent on the global state estimate. Compared with Equation (16), the dependence has been moved from attitude error model to gyroscope bias error model.

Since the attitude error element of ${\textbf{d}}{{\textbf{x}}_{\textrm{left}}}$ has the same definition as that of ${\boldsymbol{\delta} }{{\textbf{x}}_{\textrm{ref}}}$, the measurement transition matrix corresponding to ${\textbf{d}}{{\textbf{x}}_{\textrm{left}}}$ is the same as Equation (17) or (19). In QRIEKF, the measurement transition matrix in Equation (19) is used. In Gui and de Ruiter (Reference Gui and de Ruiter2018), such a trajectory-independent measurement transition matrix is derived from the invariance theory. In this paper, we present another perspective on the derivation of the trajectory-independent measurement transition matrix as shown in Equations (17)–(19).

5. Application aspects

The explicit MEKF procedures are summarised in Table 1. The transition matrices ${\textbf{F}}(t )$, ${\textbf{G}}(t )$ and ${{\textbf{H}}_k}$ in Table 1 take different forms, as in Equations (11)–(13), (16)–(19), (25) and (28), according to different error state definitions. GEKF and QRIEKF have also been viewed as types of MEKF. The essential difference between the different algorithms lies in the error state definition. Different error state definitions result in different state-space models and different attitude update procedures.

Table 1. MEKF for attitude estimation using vector observations

The retraction operation $\mathrm{\boxplus }$ denotes how to update the global state using the local error state estimate. Specifically, for error state definition ${\boldsymbol{\delta} }{{\textbf{x}}_{\textrm{body}}}$, ${\boldsymbol{\Delta} }{\hat{{\textbf{x}}}_k} = {\left[ {\begin{array}{@{}cc@{}} {{\boldsymbol{\delta} \hat{\alpha }}{{_k^{b + }}^T}}& {{\boldsymbol{\delta} \hat{\beta }}{{_k^ + }^T}} \end{array}} \right]^T}$, the retraction operation is given by

(29a)\begin{gather}\hat{{\textbf{q}}}_k^ +{=} {exp _{\textbf{q}}}({{\boldsymbol{\delta} \hat{\alpha }}_k^{b + }} )\otimes \hat{{\textbf{q}}}_k^ - \end{gather}
(29b)\begin{gather}\hat{{\boldsymbol{\beta} }}_k^ +{=} \hat{{\boldsymbol{\beta} }}_k^ -{+} {\boldsymbol{\delta} \hat{\beta }}_k^ +\end{gather}

where

(30)\begin{equation}{exp _{\textbf{q}}}({{\boldsymbol{\delta} \alpha }} )= \left[ {\begin{array}{@{}c@{}} {\dfrac{{{\boldsymbol{\delta} \alpha }}}{{||{{\boldsymbol{\delta} \alpha }} ||}}\sin ({{{||{{\boldsymbol{\delta} \alpha }} ||} / 2}} )}\\ {\cos ({{{||{{\boldsymbol{\delta} \alpha }} ||} / 2}} )} \end{array}} \right]\end{equation}

For error state definition ${\boldsymbol{\delta} }{{\textbf{x}}_{\textrm{ref}}}$, ${\boldsymbol{\Delta} }{\hat{{\textbf{x}}}_k} = {\left[ {\begin{array}{@{}cc@{}} {{\boldsymbol{\delta} \hat{\alpha }}{{_k^{r + }}^T}}& {{\boldsymbol{\delta} \hat{\beta }}{{_k^ + }^T}} \end{array}} \right]^T}$, the retraction operation is given by

(31a)\begin{gather}\hat{{\textbf{q}}}_k^ += \hat{{\textbf{q}}}_k^ -{\otimes} {exp _{\textbf{q}}}({{\boldsymbol{\delta} \hat{\alpha }}_k^{r + }} )\end{gather}
(31b)\begin{gather}\hat{{\boldsymbol{\beta} }}_k^ + = \hat{{\boldsymbol{\beta} }}_k^ -{+} {\boldsymbol{\delta} \hat{\beta }}_k^ +\end{gather}

For error state definition ${\textbf{d}}{{\textbf{x}}_{\textrm{right}}}$, ${\boldsymbol{\Delta} }{\hat{{\textbf{x}}}_k} = {\left[ {\begin{array}{@{}cc@{}} {{\textbf{d}\hat{\alpha }}{{_k^{b + }}^T}}& {{\textbf{d}\hat{\beta }}{{_k^{b + }}^T}} \end{array}} \right]^T}$, the retraction operation is given by

(32a)\begin{gather}\hat{{\textbf{q}}}_k^ + = {exp _{\textbf{q}}}({{\textbf{d}\hat{\alpha }}_k^{b + }} )\otimes \hat{{\textbf{q}}}_k^ - \end{gather}
(32b)\begin{gather}\hat{{\boldsymbol{\beta} }}_k^ + = \hat{{\boldsymbol{\beta} }}_k^ -{+} {\textbf{d}\hat{\beta }}_k^{b + } + ({\hat{{\boldsymbol{\beta} }}_k^ -{\times} } ){\textbf{d}\hat{\alpha }}_k^{b + }\end{gather}

For error state definition ${\textbf{d}}{{\textbf{x}}_{\textrm{left}}}$, ${\boldsymbol{\Delta} }{\hat{{\textbf{x}}}_k} = {\left[ {\begin{array}{@{}cc@{}} {{\textbf{d}\hat{\alpha }}{{_k^{r + }}^T}}& {{\textbf{d}\hat{\beta }}{{_k^{r + }}^T}} \end{array}} \right]^T}$, the retraction operation is given by

(33a)\begin{gather}\hat{{\textbf{q}}}_k^ + = \hat{{\textbf{q}}}_k^ -{\otimes} {exp _{\textbf{q}}}({{\textbf{d}\hat{\alpha }}_k^{r + }} )\end{gather}
(33b)\begin{gather}\hat{{\boldsymbol{\beta} }}_k^ + = \hat{{\boldsymbol{\beta} }}_k^ -{+} {\textbf{A}}(\hat{{\textbf{q}}}_k^ - ){\textbf{d}\hat{\beta }}_k^{r + }\end{gather}

It should be noted that for error state ${\boldsymbol{\delta} }{{\textbf{x}}_{\textrm{ref}}}$ and ${\textbf{d}}{{\textbf{x}}_{\textrm{left}}}$, if making use of the global state-independent transition matrix in Equation (19), the transformed innovation in Equation (20) and measurement noise covariance in Equation (21) should be used accordingly.

It should be noted that, in the aforementioned methods, the covariance update has not been incorporated for the attitude update. One can refer to Mueller et al. (Reference Mueller, Hehn and Andrea2017) for the corresponding covariance update methods.

6. Numerical example

In this section, MEKF with different state error definitions and different state-space models are evaluated via the example of simulated spacecraft attitude estimation. The following six algorithms are evaluated and compared.

1. The traditional MEKF, that is, with state-space model from Equations (11) and (12). In this algorithm, the rigorous error quaternion construction from Equation (30) has been used. In this respect, here the MEKF is just the QLIEKF in Gui and de Ruiter (Reference Gui and de Ruiter2018).

2. MEKF with state-space model from Equations (11) and (13), denoted as invariant MEKF (IMEKF). Here we make use of the terminology ‘invariant’ to express that the measurement transition matrix is independent of the global state estimate.

3. MEKF with state-space model from Equations (25) and (12), denoted as GEKF. This state-space model is just the same as that in GEKF. So, we have made use of the same terminology ‘GEKF’ in Andrle and Crassidis (Reference Andrle and Crassidis2015).

4. MEKF with state-space model from Equations (25) and (13), denoted as invariant GEKF (IGEKF).

5. MEKF with state-space model from Equations (16) and (19), denoted as MEKF-ref. In this algorithm, the attitude error is expressed in reference frame.

6. MEKF with state-space model from Equations (28) and (19), denoted as QRIEKF. In this algorithm, the attitude error is expressed in reference frame. This is just the filter developed in Gui and de Ruiter (Reference Gui and de Ruiter2018), so we make use of the same terminology.

The numerical example used is the same as that in Gui and de Ruiter (Reference Gui and de Ruiter2018). The simulation parameters are all the same as those in Gui and de Ruiter (Reference Gui and de Ruiter2018) for a freely tumbling spacecraft in a circular orbit. The spacecraft angular velocity is measured by the gyroscopes at a frequency of 0⋅1 Hz. The vector observations are provided by the sun sensor and magnetometer. The sampling frequency of the two sensors is 1 Hz. The 12th Generation (IGRF-12) model is used to provide the reference magnetic field vector.

First, a scenario with large initial estimation errors is considered. The noise parameters for the gyroscope measurements are given by ${{\boldsymbol{\eta} }_{\textbf{u}}} = \sqrt {10} \times {10^{ - 10}}$ and ${{\boldsymbol{\eta} }_{\textbf{v}}} = \sqrt {10} \times {10^{ - 7}}$. The covariance of sun sensor noise is set to 0⋅01752I3×3 and the covariance of magnetometer noise is set to 0⋅08732I3×3. The true initial attitude and gyroscope bias are generated randomly according to ${\boldsymbol{\alpha} }(0)\sim N({{\textbf{0}}_{3 \times 1}},{({150^ \circ })^2}{{\textbf{I}}_{3 \times 3}})$ and ${\boldsymbol{\beta} }(0)\sim N({{\textbf{0}}_{3 \times 1}},{{{({{20}^ \circ }} / {h)}}^2}{{\textbf{I}}_{3 \times 3}})$. For each filter, the initial attitude error covariance is set as ${({150^ \circ })^2}$ for each axis and the initial bias error covariance is set as ${{{({{20}^ \circ }} / {h)}}^2}$. The initial bias estimate is set to zero. The initial attitude quaternion estimate is set to unit quaternion. A total of 100 Monte Carlo runs are conducted, each of which spans 60 min. The root mean squared errors (RMSEs) of the attitude and gyroscope bias estimates by different filters are shown in Figures 1 and 2, respectively. It is shown that all the filters with trajectory-independent measurement models have a good performance in terms of both convergence speed and steady-state estimation accuracy. All of them demonstrate significantly better performance than the traditional MEKF and GEKF. In this simulation scenario, the superiority of GEKF over MEKF has not been observed. It can be said that the positive effect provided by the common frame treatment of the gyroscope bias has been counteracted by the negative effect of the trajectory-dependent measurement model. According to the difference between MEKF and IMEKF, it can be concluded that the state prediction-dependent measurement model in MEKF has a negative effect on the filtering performance. For the four filters with trajectory-independent measurement models, their process models are all trajectory-dependent due to the incorporation of the gyroscope bias as state element. However, regarding the attitude part, the corresponding model is trajectory-independent. This is just the primary reason why these filters can still work well for large initialisation errors with the linear Kalman filtering framework. There are slight differences in the steady-state estimates between the four filters with trajectory-independent measurement models. MEKF-ref and QRIEKF perform a little better than IMEKF and IGEKF. This difference is caused by the incorporation form of the gyroscope drift bias estimate in the process models. For MEKF-ref and QRIEKF, the relation between the attitude error and gyroscope bias error is linear as shown in Equations (16) and (28), while it is nonlinear for IMEKF and IGEKF as shown in Equations (11) and (25). However, the performance of MEKF-ref and QRIEKF is of the same grade, so it cannot be concluded which one of $\mathrm{\mathbb{S}\mathbb{E}}(3 )$ and $\mathrm{\mathbb{S}{\mathbb O}}(3 )$ is better than the other. In previous works, the $\mathrm{\mathbb{S}\mathbb{E}}(3 )$ state formulation is always argued to be better than the $\mathrm{\mathbb{S}{\mathbb O}}(3 )$ formulation. This is because with the $\mathrm{\mathbb{S}\mathbb{E}}(3 )$ formulation, the resultant state-space model is trajectory-independent, while this is not the case for the $\mathrm{\mathbb{S}{\mathbb O}}(3 )$ formulation. However, for the spacecraft attitude estimate problem, no matter the $\mathrm{\mathbb{S}\mathbb{E}}(3 )$ or $\mathrm{\mathbb{S}{\mathbb O}}(3 )$ formulation, the attitude error models are both trajectory-independent if the gyroscope bias is not considered. Although IGEKF performs better than IMEKF for attitude estimation in this case, IMEKF performs better than IGEKF in the next case. Therefore, we cannot judge which one is superior or inferior.

Figure 1. RMSEs of attitude estimate errors for large initial estimation errors

Figure 2. RMSEs of gyroscope drift bias estimate errors for large initial estimation errors

Second, the severe case with a small initial error covariance but extremely large initial state error is considered. The noise parameters for the gyroscope measurements are given by ${{\boldsymbol{\eta} }_{\textbf{u}}} = \sqrt {10} \times {10^{ - 8}}$ and ${{\boldsymbol{\eta} }_{\textbf{v}}} = \sqrt {10} \times {10^{ - 5}}$. The covariance of sun sensor noise and magnetometer noise are the same as the first case. The initial attitude quaternion estimate is still set to unit quaternion and the initial bias estimate is still set to zero. But the initial true gyroscope bias is set as ${\boldsymbol{\beta} }(0) = {{{{[{100,10,10} ]}^ \circ }} / h}$ and the initial true attitude quaternion is set as ${\textbf{q}}(0) = {[{1,0,0,0} ]^T}$, which means that the initial attitude error is ${180^ \circ }$. For each filter, the initial attitude error covariance is set as ${({10^ \circ })^2}$ for each axis and the initial bias error covariance is set as ${{{({5^ \circ }} / {h)}}^2}$. A total of 100 Monte Carlo runs are conducted, each of which spans 80 min. The RMSEs of the attitude and gyroscope bias estimates by these filters are shown in Figures 3 and 4, respectively. The results are consistent with those in the last case. All the filters with trajectory-independent measurement models perform much better than the traditional MEKF. This further demonstrates the importance of the trajectory-independent measurement models. In this case, MEKF-ref and QRIEKF still perform a little better than IMEKF and IGEKF for attitude estimation. However, for gyroscope bias estimation, IMEKF performs the best. For attitude estimation, IMEKF performs a little better than IGEKF, which is opposite to the last case. This further indicates that $\mathrm{\mathbb{S}\mathbb{E}}(3 )$ or $\mathrm{\mathbb{S}{\mathbb O}}(3 )$ are only two different state formulations and there is no definitive conclusion as to which one is better. Their performance in filtering is problem-dependent.

Figure 3. RMSEs of attitude estimate errors for a severe initial condition

Figure 4. RMSEs of gyroscope drift bias estimate errors for a severe initial condition

7. Conclusion

In this paper, the MEKF is revisited for attitude estimation with vector observations making use of group Lie theory. It is pointed out that the dynamic model of the attitude estimation is group-affine and, therefore, the corresponding error state model will be trajectory-independent. With the trajectory-independent linear state-space models, it is possible to cope with large initialisation errors even within the linear Kalman filtering framework. However, the measurement model used in existing MEKF is dependent on the attitude prediction, which is inconsistent with invariant theory. Substituting the attitude prediction related vectors with the vector observations in the body frame, the resultant measurement model is now trajectory-independent. Meanwhile, the measurement model with reference frame attitude error definition can also be transformed into trajectory-independent form. Monte Carlo simulations were conducted to evaluate the MEKF variants with different state-space models. The results have shown that the performance of MEKF can be much improved with the trajectory-independent measurement models in some severe scenarios such as large initial estimation errors or erroneous initial error covariance.

Funding statement

This research was supported by the National Natural Science Foundation of China under the project 61873275.

Acknowledgements

The author would like to thank Prof. Haichao Gui from the School of Astronautics, Beihang University, for providing the numerical examples codes.

Appendix

In this Appendix, we will derive the attitude error state model without any approximation as that in Equations (10) and (15). First, we will derive the attitude error state model in the body frame.

The attitude error in body frame is defined in Equation (9). Taking derivative of both sides of Equation (9) gives

(A1)\begin{align} \dfrac{{d{\textbf{A}}({\boldsymbol{\delta} q})}}{{dt}} & =\dot{{\textbf{A}}}({\textbf{q}}){\textbf{A}}{(\hat{{\textbf{q}}})^{ - 1}} - {\textbf{A}}({\textbf{q}}){\textbf{A}}{(\hat{{\textbf{q}}})^{ - 1}}\dot{{\textbf{A}}}(\hat{{\textbf{q}}}){\textbf{A}}{(\hat{{\textbf{q}}})^{ - 1}}\nonumber\\ & =- ({{\boldsymbol{\omega} } \times } ){\textbf{A}}({\textbf{q}} ){\textbf{A}}{(\hat{{\textbf{q}}})^{ - 1}} + {\textbf{A}}({\textbf{q}}){\textbf{A}}{(\hat{{\textbf{q}}})^{ - 1}}({{\boldsymbol{\omega} } \times } )\\ & =- ({{\boldsymbol{\omega} } \times } ){\textbf{A}}({\boldsymbol{\delta} q}) + {\textbf{A}}({\boldsymbol{\delta} q})({{\boldsymbol{\omega} } \times } )\nonumber\end{align}

Suppose the initial vector-form error corresponds to ${\textbf{A}}({{\boldsymbol{\delta} q}(0)} )$ as ${\boldsymbol{\delta} }{{\boldsymbol{\alpha} }^b}(0)$. They are related to each other through the exponential map as

(A2)\begin{equation}{\textbf{A}}({{\boldsymbol{\delta} q}(0)} )= exp [{{\boldsymbol{\delta} }{{\boldsymbol{\alpha} }^b}(0)} ]= {exp _\textrm{m}}[{{\boldsymbol{\delta} }{{\boldsymbol{\alpha} }^b}(0) \times } ]\end{equation}

Define the following attitude error

(A3)\begin{equation}{\textbf{A}}({\boldsymbol{\delta} q}) = {\textbf{A}}({\textbf{q}}){\textbf{A}}({{\boldsymbol{\delta} q}(0)} ){\textbf{A}}{({\textbf{q}})^{ - 1}}\end{equation}

Taking the derivative of both sides of Equation (A3) gives

(A4)\begin{align} \dfrac{{d{\textbf{A}}({\boldsymbol{\delta} q})}}{{dt}} & =\dot{{\textbf{A}}}({\textbf{q}}){\textbf{A}}({{\boldsymbol{\delta} q}(0)} ){\textbf{A}}{({\textbf{q}})^{ - 1}} - {\textbf{A}}({\textbf{q}}){\textbf{A}}({{\boldsymbol{\delta} q}(0)} ){\textbf{A}}{({\textbf{q}})^{ - 1}}\dot{{\textbf{A}}}({\textbf{q}}){\textbf{A}}{({\textbf{q}})^{ - 1}}\nonumber\\ & =- ({{\boldsymbol{\omega} } \times } ){\textbf{A}}({\textbf{q}}){\textbf{A}}({{\boldsymbol{\delta} q}(0)} ){\textbf{A}}{({\textbf{q}})^{ - 1}} + {\textbf{A}}({\textbf{q}}){\textbf{A}}({{\boldsymbol{\delta} q}(0)} ){\textbf{A}}{({\textbf{q}})^{ - 1}}({{\boldsymbol{\omega} } \times } ){\textbf{A}}({\textbf{q}}){\textbf{A}}{({\textbf{q}})^{ - 1}}\\ & =- ({{\boldsymbol{\omega} } \times } ){\textbf{A}}({\boldsymbol{\delta} q}) + {\textbf{A}}({\boldsymbol{\delta} q})({{\boldsymbol{\omega} } \times } )\nonumber\end{align}

which means that the defined attitude error in Equation (A2) is a solution of the attitude error model in Equation (A1).

Suppose the initial vector-form error corresponds to ${\textbf{A}}({{\boldsymbol{\delta} q}} )$ as ${\boldsymbol{\delta} }{{\boldsymbol{\alpha} }^b}$. According to the relationship between the matrix Lie group and its associated Lie algebra, Equation (A3) can be rewritten as

(A5)\begin{equation}exp ({{\boldsymbol{\delta} }{{\boldsymbol{\alpha} }^b}} )= {\textbf{A}}({\textbf{q}})exp [{{\boldsymbol{\delta} }{{\boldsymbol{\alpha} }^b}(0)} ]{\textbf{A}}{({\textbf{q}})^{ - 1}} = exp [{{\textbf{A}}({\textbf{q}}){\boldsymbol{\delta} }{{\boldsymbol{\alpha} }^b}(0)} ]\end{equation}

That is, we have

(A6)\begin{equation}{\boldsymbol{\delta} }{{\boldsymbol{\alpha} }^b} = {\textbf{A}}({\textbf{q}}){\boldsymbol{\delta} }{{\boldsymbol{\alpha} }^b}(0)\end{equation}

Taking the derivative of both sides of Equation (A6) gives

(A7)\begin{equation}{\boldsymbol{\delta} }{\dot{{\boldsymbol{\alpha} }}^b} ={-} ({{\boldsymbol{\omega} } \times } ){\textbf{A}}({\textbf{q}}){\boldsymbol{\delta} }{{\boldsymbol{\alpha} }^b}(0) ={-} ({{\boldsymbol{\omega} } \times } ){\boldsymbol{\delta} }{{\boldsymbol{\alpha} }^b}\end{equation}

which is just the attitude error model involved in Equation (11). From Equation (A1) to (A7), it is shown that there is no approximation when deriving the attitude error model. That is to say, the attitude error model is also applicable with large initial attitude error.

Similarly, we now derive the attitude error model in reference frame. Taking the derivative of both sides of Equation (14) gives

(A8)\begin{align} \dfrac{{d{\textbf{A}}({{\boldsymbol{\delta} q}} )}}{{dt}} & =- {\textbf{A}}{({\hat{{\textbf{q}}}} )^{ - 1}}\dot{{\textbf{A}}}({\hat{{\textbf{q}}}} ){\textbf{A}}{({\hat{{\textbf{q}}}} )^{ - 1}}{\textbf{A}}({\textbf{q}} )+ {\textbf{A}}{({\hat{{\textbf{q}}}} )^{ - 1}}\dot{{\textbf{A}}}({\textbf{q}} )\nonumber\\ & ={\textbf{A}}{({\hat{{\textbf{q}}}} )^{ - 1}}({{\boldsymbol{\omega} } \times } ){\textbf{A}}({\hat{{\textbf{q}}}} ){\textbf{A}}({{\boldsymbol{\delta} q}} )- {\textbf{A}}{({\hat{{\textbf{q}}}} )^{ - 1}}({{\boldsymbol{\omega} } \times } ){\textbf{A}}({\textbf{q}} )\\ & ={\textbf{A}}{({\hat{{\textbf{q}}}} )^{ - 1}}({{\boldsymbol{\omega} } \times } ){\textbf{A}}({\hat{{\textbf{q}}}} ){\textbf{A}}({{\boldsymbol{\delta} q}} )- {\textbf{A}}{({\hat{{\textbf{q}}}} )^{ - 1}}({{\boldsymbol{\omega} } \times } ){\textbf{A}}({\hat{{\textbf{q}}}} ){\textbf{A}}{({\hat{{\textbf{q}}}} )^{ - 1}}{\textbf{A}}({\textbf{q}} )\nonumber\\ & ={{\textbf{0}}_{3 \times 3}}\nonumber\end{align}

Suppose the initial vector-form error corresponding to ${\textbf{A}}({{\boldsymbol{\delta} q}} )$ is ${\boldsymbol{\delta} }{{\boldsymbol{\alpha} }^r}$. They are related to each other through the exponential map as

(A9)\begin{equation}{\textbf{A}}({{\boldsymbol{\delta} q}} )= exp ({{\boldsymbol{\alpha} }^r}) = {exp _\textrm{m}}({{\boldsymbol{\alpha} }^r} \times )\end{equation}

It can be easily deduced from Equation (A8) and (A9) that

(A10)\begin{equation}{\dot{{\boldsymbol{\alpha} }}^r} = {{\textbf{0}}_{3 \times 1}}\end{equation}

which is just the attitude error model involved in Equation (16). From Equation (A8) to (A10), it is shown that there is no approximation when deriving the attitude error model. That is to say, the attitude error model is also applicable with large initial attitude error.

References

Andrle, M. S. and Crassidis, J. L. (2015). Attitude estimation employing common frame error representations. Journal of Guidance Control and Dynamics, 38(9), 1614–1624.CrossRefGoogle Scholar
Barrau, A. (2015). Non-linear state error based extended Kalman filters with applications to navigation. Thesis, MINES Paristech, Paris, France.Google Scholar
Barrau, A. and Bonnabel, S. (2014). Intrinsic filtering on Lie groups with applications to attitude estimation. IEEE Transactions on Automatic Control, 60(2), 436–449.CrossRefGoogle Scholar
Barrau, A. and Bonnabel, S. (2015). An EKF-SLAM algorithm with consistency properties. arXiv preprint, arXiv:1510.06263.Google Scholar
Barrau, A. and Bonnabel, S. (2017). The invariant extended Kalman filter as a stable observer. IEEE Transactions on Automatic Control, 62(4), 1797–1812.CrossRefGoogle Scholar
Chang, L. B. (2020). SE(3) based extended Kalman filter for spacecraft attitude estimation. arXiv preprint, arXiv:2003.12978.Google Scholar
Cheng, Y. and Crassidis, J. L. (2010). Particle filtering for attitude estimation using a minimal local-error representation. Journal of Guidance, Control, and Dynamics, 33(4), 1305–1310.10.2514/1.47236CrossRefGoogle Scholar
Crassidis, J. L. and Junkins, J. L. (2012). Optimal Estimation of Dynamic Systems (2nd ed.). Boca Raton, FL: CRC Press, Chap. 3, pp. 153–154, Chap. 7, pp. 421–424.Google Scholar
Crassidis, J. L. and Markley, F. L. (2003). Unscented filtering for spacecraft attitude estimation. Journal of Guidance, Control, and Dynamics, 26(4), 536–542.CrossRefGoogle Scholar
Crassidis, J. L., Markley, F. L. and Cheng, Y. (2007). Survey of nonlinear attitude estimation methods. Journal of Guidance, Control, and Dynamics, 30(1), 12–28.CrossRefGoogle Scholar
Cui, J., Wang, M., Wu, W. and He, X. (2021). Lie group based nonlinear state errors for MEMS-IMU/GNSS/magnetometer integrated navigation. The Journal of Navigation, 1–14. doi:10.1017/S037346332100014XGoogle Scholar
Gai, E., Daly, K., Harrison, J. and Lemos, L. (1985). Star-sensor-based satellite attitude/attitude rate estimator. Journal of Guidance, Control, and Dynamics, 8(5), 560–565.CrossRefGoogle Scholar
Gui, H. and de Ruiter, A. H. J. (2018). Quaternion invariant extended Kalman filtering for spacecraft attitude estimation. Journal of Guidance, Control, and Dynamics, 41(4), 863–878.CrossRefGoogle Scholar
Hartley, R., Ghaffari, M., Eustice, R. M. and Grizzle, J. W. (2020). Contact-aided invariant extended Kalman filtering for robot state estimation. The International Journal of Robotics Research, 39(4), 402–430.CrossRefGoogle Scholar
Lefferts, E. J., Markley, F. L. and Shuster, M. D. (1982). Kalman filtering for spacecraft attitude estimation. Journal of Guidance, Control, and Dynamics, 5(5), 417–429.CrossRefGoogle Scholar
Li, M. and Mourikis, A. I. (2012). Improving the Accuracy of EKF-Based Visual-Inertial Odometry. 2012 IEEE International Conference on Robotics and Automation, St. Paul, MN, 828–835.CrossRefGoogle Scholar
Markley, F. L. (2003). Attitude error representations for Kalman filtering. Journal of Guidance, Control, and Dynamics, 26(2), 311–317.10.2514/2.5048CrossRefGoogle Scholar
Markley, F. L. and Crassidis, J. L. (2014). Fundamentals of Spacecraft Attitude Determination and Control. New York: Microcosm Press and Springer.CrossRefGoogle Scholar
Mueller, M. W., Hehn, M. and Andrea, R. (2017). Covariance correction step for Kalman filtering with an attitude. Journal of Guidance, Control, and Dynamics, 40(9), 2301–2306.10.2514/1.G000848CrossRefGoogle Scholar
Wang, M., Wu, W., He, X., Li, Y. and Pan, X. (2019). Consistent ST-EKF for long distance land vehicle navigation based on SINS/OD integration. IEEE Transactions on Vehicular Technology, 68(11), 10525–10534.CrossRefGoogle Scholar
Wang, M., Wu, W., Zhou, P. and He, X. (2018). State transformation extended Kalman filter for GPS/SINS tightly coupled integration. GPS Solutions, 22(4), 1–12.10.1007/s10291-018-0773-3CrossRefGoogle Scholar
Figure 0

Table 1. MEKF for attitude estimation using vector observations

Figure 1

Figure 1. RMSEs of attitude estimate errors for large initial estimation errors

Figure 2

Figure 2. RMSEs of gyroscope drift bias estimate errors for large initial estimation errors

Figure 3

Figure 3. RMSEs of attitude estimate errors for a severe initial condition

Figure 4

Figure 4. RMSEs of gyroscope drift bias estimate errors for a severe initial condition