Derivation of All Attitude Error Governing Equations for Attitude Filtering and Control

Abstract

This article presents the full analytical derivations of the attitude error kinematics equations. This is done for several attitude error representations, obtaining compact closed-forms expressions. Attitude error is defined as the rotation between true and estimated orientations. Two distinct approaches to attitude error kinematics are developed. In the first, the estimated angular velocity is defined in the true attitude axes frame, while in the second, it is defined in the estimated attitude axes frame. The first approach is of interest in simulations where the true attitude is known, while the second approach is for real estimation/control applications. Two nonlinear kinematic models are derived that are valid for arbitrarily large rotations and rotation rates. The results presented are expected to be broadly useful to nonlinear attitude estimation/control filtering formulations. A discussion of the benefits of the derived error kinematic models is included.

1. Introduction

Many attitude representations are available for modeling problems in science and engineering [1,2,3,4,5,6,7,8,9]. Nonlinearity of the representation of a given physical motion and location of geometric singularities are dependant on (1) the true motion, (2) the attitude representation selected, and (3) the particular choice of estimated axes. Selecting the appropriate representation is highly linked with the kind of problem being considered. The most popular attitude parameterizations are as follows [1,10,11,12]:

• Direction Cosine Matrix (DCM): nine-parameter matrix representation subject to orthogonal constraint, non-singular;
• Principal axis and angle: three-parameter representation (the axis is a unit vector), singular (axis undefined when the angle is zero);
• Euler–Rodrigues parameters (quaternion): four-parameter vector representation subject to unit norm, non-singular;
• Rodrigues parameters (RP; Gibbs vector): three-parameter representation, singular (infinite values for $\pi$ rotations);
• Modified Rodrigues parameters (MRP): three-parameter representation; and
• Cayley–Klein parameters: $2\times 2$ complex matrix representation subject to unitary constraint.

Definitions, characteristics, and transformations between these representations can be found in many references [1,4,5,6,8,10,13]. For applications requiring large and rapid rotational dynamics, there exists a need for developing attitude error kinematic models that exactly describe arbitrary large rotational motions. In particular, for control problems, Markley [8] has considered different attitude error representations for estimating the state of a maneuvering spacecraft. He has clarified the relationship between the four-component quaternion and the Multiplicative Extended Kalman Filter. Cao et al. [14] developed an unscented predictive filter for satellite formation. Later, Cao et al. [15] proposed a huber-based Kalman filter for the attitude estimation problem of small satellites. Batch and Paielli [16] investigated the rigid-body attitude error using inversion techniques to obtain a robust linearized model of attitude control error dynamics. Sun et al. have derived relative rotational and transnational dynamics error for spacecraft rendezvous control [17]. Crassidis et al. [18] studied a variable-structure control strategy for maneuvering vehicles. In their work, they used a feedback linearizing technique and added an additional term to the spacecraft maneuvers to deal with model uncertainties, which they demonstrated always provides an optimal response. Ahmed et al. [19] extended previous work to consider adaptive asymptotic tracking during maneuvers while estimating inertia properties. They used a Lyapunov argument to generate an unconditionally robust control law with respect to their assumed parametric uncertainty. Bani Younes et al. [20,21] considered generalized optimal control formulations that handle nonlinear system dynamics and enable the development of control gain sensitivities to handle plant model uncertainties during maneuvers. Sharma and Tiwari [22] introduced MRP for parameterization of the orientation error. They defined the attitude error as an additive quantity. Their work is extended by retaining a rigorous nonlinear MRP-based error equation. Schaub et al. [23] developed a new penalty function for optimal control formulation of spacecraft attitude control problems. This function returns the same scalar penalty for a given physical attitude regardless of the attitude coordinate choice. A recent work by Tanygi investigated the projective geometry of vectorial attitude parameterizations with applications for control [24] and estimation [25]. Junkins [26] discussed the link between designing a good controller and the choice of coordinates to represent the attitude kinematics. He linearized the attitude error equations by defining the departure motion as an additive error from a nominal trajectory. Unfortunately, the error in orientation cannot be rigorously represented as additive (linear) because of the nonlinear behavior of underlying kinematical descriptions [2].

Error equations are challenging to define for quaternion variables because of the implied coupling effects between the quaternion components and the implicit norm constraint. The position error is linear and, therefore, can be described by the distance between the two vectors representing the true and the estimated position states. Unfortunately, the error in orientation in not linear and, therefore, must be described by a matrix product (corrective attitude matrix [2]). An exact quaternion error equation is defined as the quaternion product between the true quaternion and the inverse quaternion of the estimated (or reference) state. The significant advantage of this approach is that the norm constraint is preserved and, most importantly, that the quaternion error product remains valid for arbitrarily large relative rotations. This fact is exploited to develop equivalent arbitrarily large relative rotational representations for the vehicle attitude motion. The resulting expressions are compact, accurate, and computationally efficient.

The primary goal of this paper is to develop uniformly valid kinematics equations to describe the time-evolution of the attitude error. References [1,3,4,5,10] contain good review for the attitude kinematics equations. This paper presents compact nonlinear attitude error kinematics equations that can be used in attitude control and/or estimation dynamics problems. Exact analytical (closed-form and no approximations) attitude error kinematic equations are derived for most popular and known attitude representations. This paper extends previous work originally initiated by the authors on developing attitude error kinematics [27,28,29,30], where the estimated angular velocity is defined in the true attitude axes frame. It builds on our initial findings and extends the formulations to address more detailed developments and to present two distinct error kinematics approaches treated with more compact depth and insights in the derivations. It also presents simulation examples that demonstrate the applications in attitude filtering and tracking control. The multiplicative Kalman filter is discussed to enable the use of these attitude error kinematics for two different coordinate choices. The resulting expressions have been optimized to obtain the most compact and computationally efficient forms. Also, the applications of these formulations are discussed by solving the open-loop optimal spacecraft tracking control problem.

The major contribution of this paper is the complete development of attitude error kinematics equations where the estimated angular velocity is defined in either the true attitude axes frame or in the estimated attitude axes frame. These equations can be used for arbitrarily large relative rotations and rotation rates. These attitude errors represent rotations between estimated and true attitudes. Numerical integrations of these kinematic equations are performed to validate the machine error accuracy for each attitude representation. Singularities and constraints are discussed for minimum and non-minimum attitude parameter representations, respectively. Applications are expected in rotational dynamics problems for both nonlinear attitude estimation filtering and attitude tracking.

2. Attitude Error Kinematics

With reference to Figure 1, let C be the true attitude matrix (DCM) of which the axes are $[\mathit{x},\mathit{y},\mathit{z}]$ (dashed, black) and let $\widehat{C}$ be the estimated attitude matrix of which the axes are $[\widehat{\mathit{x}},\widehat{\mathit{y}},\widehat{\mathit{z}}]$ (dashed, red).

The relationships between the DCM with the corresponding Euler’s angles associated with the rotation axes and sequences are given in

Table A1. Angular velocity error vector and Euler angle error rates mapping.

i-j-k	${\mathcal{M}}_{\mathit{ijk}}^{-1}$	${\mathcal{M}}_{\mathit{ijk}}$	i-j-k	${\mathcal{M}}_{\mathit{ijk}}^{-1}$	${\mathcal{M}}_{\mathit{ijk}}$
1-2-1	${\displaystyle \frac{1}{s_2}}\left[\begin{array}{ccc}0& s_3& c_3\\ 0& s_2{c}_3& -s_2{c}_3\\ s_2& -c_2{s}_3& -c_2{c}_3\end{array}\right]$	$\left[\begin{array}{ccc}{c}_2& 0& 1\\ s_2{s}_3& c_3& 0\\ s_2{c}_3& -s_3& 0\end{array}\right]$	2-3-1	${\displaystyle \frac{1}{c_2}}\left[\begin{array}{ccc}0& c_3& -s_3\\ 0& c_2{s}_3& c_2{c}_3\\ c_2& -s_2{c}_3& s_2{s}_3\end{array}\right]$	$\left[\begin{array}{ccc}{s}_2& 0& 1\\ c_2{c}_3& s_3& 0\\ -c_2{s}_3& c_3& 0\end{array}\right]$
1-2-3	${\displaystyle \frac{1}{c_2}}\left[\begin{array}{ccc}{c}_3& -s_3& 0\\ c_2{s}_3& c_2{c}_3& 0\\ -s_2{c}_3& s_2{s}_3& c_2\end{array}\right]$	$\left[\begin{array}{ccc}{c}_2{c}_3& s_3& 0\\ -c_2{s}_3& c_3& 0\\ s_2& 0& 1\end{array}\right]$	2-3-2	${\displaystyle \frac{1}{s_2}}\left[\begin{array}{ccc}{c}_3& 0& s_3\\ -s_2{s}_3& 0& s_2{c}_3\\ -c_2{c}_3& s_2& -c_2{s}_3\end{array}\right]$	$\left[\begin{array}{ccc}{s}_2{c}_3& -s_3& 0\\ c_2& 0& 1\\ s_2{s}_3& c_3& 0\end{array}\right]$
1-3-1	${\displaystyle \frac{1}{s_2}}\left[\begin{array}{ccc}0& -c_3& s_3\\ 0& s_2{s}_3& s_2{c}_3\\ s_2& c_2{c}_3& -c_2{s}_3\end{array}\right]$	$\left[\begin{array}{ccc}{c}_2& 0& 1\\ -s_2{c}_3& s_3& 0\\ s_2{s}_3& c_3& 0\end{array}\right]$	3-1-2	${\displaystyle \frac{1}{c_2}}\left[\begin{array}{ccc}-s_3& 0& c_3\\ c_2{c}_3& 0& c_2{s}_3\\ s_2{s}_3& c_2& -s_2{c}_3\end{array}\right]$	$\left[\begin{array}{ccc}-c_2{s}_3& c_3& 0\\ s_2& 0& 1\\ c_2{c}_3& s_3& 0\end{array}\right]$
1-3-2	${\displaystyle \frac{1}{c_2}}\left[\begin{array}{ccc}{c}_3& 0& s_3\\ -c_2{s}_3& 0& c_2{c}_3\\ s_2{c}_3& c_2& s_2{s}_3\end{array}\right]$	$\left[\begin{array}{ccc}{c}_2{c}_3& -s_3& 0\\ -s_2& 0& 1\\ c_2{s}_3& c_3& 0\end{array}\right]$	3-1-3	${\displaystyle \frac{1}{s_2}}\left[\begin{array}{ccc}{s}_3& c_3& 0\\ s_2{c}_3& -s_2{s}_3& 0\\ -c_2{s}_3& -c_2{c}_3& s_2\end{array}\right]$	$\left[\begin{array}{ccc}{s}_3{s}_2& c_3& 0\\ s_2{c}_3& -s_3& 0\\ c_2& 0& 1\end{array}\right]$
2-1-2	${\displaystyle \frac{1}{s_2}}\left[\begin{array}{ccc}{s}_3& 0& -c_3\\ s_2{c}_3& 0& s_2{s}_3\\ -c_2{s}_3& s_2& c_2{c}_3\end{array}\right]$	$\left[\begin{array}{ccc}{s}_2{s}_3& c_3& 0\\ c_2& 0& 1\\ -s_2{c}_3& s_3& 0\end{array}\right]$	3-2-1	${\displaystyle \frac{1}{c_2}}\left[\begin{array}{ccc}0& 0& c_3\\ 0& c_2{c}_3& -c_2{s}_3\\ c_2& s_2{s}_3& s_2{c}_3\end{array}\right]$	$\left[\begin{array}{ccc}-s_2& 0& 1\\ c_2{s}_3& c_3& 0\\ c_2{c}_3& -s_3& 0\end{array}\right]$
2-1-3	${\displaystyle \frac{1}{c_2}}\left[\begin{array}{ccc}{s}_3& c_3& 0\\ c_2{c}_3& -c_2{s}_3& 0\\ s_2{s}_3& s_2{c}_3& c_2\end{array}\right]$	$\left[\begin{array}{ccc}{c}_2{s}_3& c_3& 0\\ c_2{c}_3& -s_3& 0\\ -s_2& 0& 1\end{array}\right]$	3-2-3	${\displaystyle \frac{1}{s_2}}\left[\begin{array}{ccc}-c_3& s_3& 0\\ s_2{s}_3& s_2{c}_3& 0\\ c_2{c}_3& -c_2{s}_3& s_2\end{array}\right]$	$\left[\begin{array}{ccc}-s_2{c}_3& s_3& 0\\ s_2{s}_3& c_3& 0\\ c_2& 0& 1\end{array}\right]$

. The attitude error is described by the corrective attitude matrix:

$$\delta C=C{\widehat{C}}^{\mathrm{T}}.$$

(1)

In fact, the product between $\delta C$ and the estimated attitude $\widehat{C}$ provides the true attitude:

$$\delta C\widehat{C}=(C{\widehat{C}}^{\mathrm{T}})\widehat{C}=C.$$

In particular, matrix $\delta C$ is the transformation matrix for vectors from the estimated $[\widehat{\mathit{x}},\widehat{\mathit{y}},\widehat{\mathit{z}}]$ frame to the true $[\mathit{x},\mathit{y},\mathit{z}]$ frame.

Let $\mathit{\omega }$ be the true angular velocity vector (black solid in Figure 1) of which the elements are defined in the $[\mathit{x},\mathit{y},\mathit{z}]$ axes of the true attitude. Now, consider $\widehat{\mathit{\omega }}$ to be the estimate of the true angular velocity vector (red solid in Figure 1). Two distinct cases appears:

(1)

The estimated $\widehat{\mathit{\omega }}$ is defined in the true body axes frame ($\mathit{x},\mathit{y},\mathit{z}$). In this case, $\mathit{\omega }$ and $\widehat{\mathit{\omega }}$ are defined in the same reference frame. Therefore, the angular velocity error vector is as follows:

$$\delta \mathit{\omega }=\mathit{\omega }-\widehat{\mathit{\omega }}.$$

(2)

This case finds application in simulations when the true attitude is known.

(2)

The estimated $\widehat{\mathit{\omega }}$ is defined in the estimated body axes frame ($\widehat{\mathit{x}},\widehat{\mathit{y}},\widehat{\mathit{z}}$). In this case, $\mathit{\omega }$ and $\widehat{\mathit{\omega }}$ are defined in two different reference frames. Therefore, the angular velocity error vector is as follows:

$$\delta \mathit{\omega }=\mathit{\omega }-\delta C\widehat{\mathit{\omega }},$$

(3)

where $\delta C$ is provided by Equation (1). This case is important in real applications as the true attitude is always unknown and the estimated angular velocity can be defined in the estimated attitude frame only.

The true attitude dynamics is defined by $\mathit{\omega }$ and C. They satisfy the DCM kinematic equations:

$$[\mathit{\omega }\times ]=-\dot{C}{C}^{\mathrm{T}}.$$

The estimated angular velocity satisfies the dynamic equation:

$$I\dot{\widehat{\mathit{\omega }}}=-[\widehat{\mathit{\omega }}\times ]I\widehat{\mathit{\omega }}+\mathit{u},$$

where $I\in {\Re }^{3\times 3}$ is the moments of inertia tensor and $\mathit{u}\in {\Re }^3$ is the torque acting on the system. We assume that the system parameters are deterministic.

To develop the attitude error kinematics equations, two distinct analysis must be performed. These two derivations are associated to the two angular velocity errors definitions provided in Equations (2) and (3), respectively. The following two sections derive the attitude error kinematics equations valid for arbitrarily large angles and rate errors.

2.1. Simulation Case: Estimated Angular Velocity Given in the True Attitude Frame

In this section, the estimated and the true angular velocities are expressed in same coordinate frames, $\delta \mathit{\omega }=\mathit{\omega }-\widehat{\mathit{\omega }}$. This approach is suitable in simulations where the true body attitude is known. When using the same reference frame for true and estimated angular velocities, the angular velocity error dynamics equation is written as follows [21,27,28]:

$$\delta \dot{\mathit{\omega }}=-I^{-1}\{[\widehat{\mathit{\omega }}\times ]I-[(I\widehat{\mathit{\omega }})\times ]\}\delta \mathit{\omega }-I^{-1}[(\delta \mathit{\omega })\times ]I\delta \mathit{\omega }+I^{-1}\mathit{u}-\dot{\widehat{\mathit{\omega }}}-I^{-1}[\widehat{\mathit{\omega }}\times ]I\widehat{\mathit{\omega }}.$$

(4)

In the following subsections, the attitude error kinematic equations are derived for the most important attitude representations and for arbitrarily large rotational angles and angular rates. These equations are mathematically simple and compact. They can be used, for instance, to validate novel control theories and/or attitude estimation filtering techniques.

2.1.1. Quaternion Error Kinematics

The quaternion error is a four-dimensional vector defined as follows:

$$\delta \mathit{q}=\left\{\begin{array}{c}\delta {\mathit{q}}_v\\ \delta q_4\end{array}\right\},$$

where $\delta {\mathit{q}}_v={\{\delta q_1,\delta q_2,\delta q_3\}}^{\mathrm{T}}=\mathit{e}sin\left({\displaystyle \frac{\varphi }{2}}\right)$, $\delta q_4=cos\left({\displaystyle \frac{\varphi }{2}}\right)$, $\mathit{e}$ is the principal axis, and $\varphi$ is the principal angle. The kinematic solution for the true quaternion trajectory defines the desired relative rotational motion for the spacecraft. Equation (1) written in term of quaternion is as follows:

$$\delta \mathit{q}=\mathit{q}\otimes {\widehat{\mathit{q}}}^{-1},$$

(5)

where ${\widehat{\mathit{q}}}^{-1}$ is the inverse of the estimated quaternion rotation and ⊗ represents the quaternion product. Note that the error, $\delta \mathit{q}$, is also a quaternion, that is, a unit vector representing the rotation from the estimated axes to the true axes. This expression is valid for arbitrarily large rational motions and provides a foundation for developing all other kinematic variable generalizations presented. We follow reference [1] in writing the quaternion product as follows:

$${\mathit{q}}^{\prime }\otimes \mathit{q}=\left\{\begin{array}{c}{q}_4^{\prime }{\mathit{q}}_v+{\mathit{q}}_v^{\prime }{q}_4-{\mathit{q}}_v^{\prime }\times {\mathit{q}}_v\\ q_4^{\prime }{q}_4-{\mathit{q}}_v^{\prime }·{\mathit{q}}_v\end{array}\right\},$$

where ${\mathit{q}}^{\prime }$ and $\mathit{q}$ represent two arbitrary quaternions. The quaternion error rate is as follows:

$$\delta \dot{\mathit{q}}=\dot{\mathit{q}}\otimes {\widehat{\mathit{q}}}^{-1}+\mathit{q}\otimes {\dot{\widehat{\mathit{q}}}}^{-1}.$$

(6)

Quaternion kinematics evolves according to the kinematic equation:

$$\dot{\mathit{q}}={\displaystyle \frac{1}{2}}\left\{\begin{array}{c}\mathit{\omega }\\ 0\end{array}\right\}\otimes \mathit{q}={\displaystyle \frac{1}{2}}\left[\begin{array}{cc}-[\mathit{\omega }\times ]& \mathit{\omega }\\ -{\mathit{\omega }}^{\mathrm{T}}& 0\end{array}\right]\mathit{q}={\displaystyle \frac{1}{2}}\Omega (\mathit{\omega })\mathit{q}.$$

(7)

The derivative of the identity $\widehat{\mathit{q}}\otimes {\widehat{\mathit{q}}}^{-1}={\{0,0,0,1\}}^{\mathrm{T}}$ leads to $\dot{\widehat{\mathit{q}}}\otimes {\widehat{\mathit{q}}}^{-1}+\widehat{\mathit{q}}\otimes {\dot{\widehat{\mathit{q}}}}^{-1}=\mathit{0}$. Hence, the inverse quaternion evolves:

$${\dot{\widehat{\mathit{q}}}}^{-1}=-{\displaystyle \frac{1}{2}}{\widehat{\mathit{q}}}^{-1}\otimes \left\{\begin{array}{c}\widehat{\mathit{\omega }}\\ 0\end{array}\right\}=-{\displaystyle \frac{1}{2}}\left[\begin{array}{cc}[\widehat{\mathit{\omega }}\times ]& \widehat{\mathit{\omega }}\\ -{\widehat{\mathit{\omega }}}^{\mathrm{T}}& 0\end{array}\right]{\widehat{\mathit{q}}}^{-1}=-{\displaystyle \frac{1}{2}}\Gamma (\widehat{\mathit{\omega }}){\widehat{\mathit{q}}}^{-1},$$

(8)

where $\Gamma (\widehat{\mathit{\omega }})$ is the estimated angular velocity matrix. Substituting Equation (7) and Equation (8) into Equation (6), yields the following:

$$\delta \dot{\mathit{q}}={\displaystyle \frac{1}{2}}\Omega (\mathit{\omega })\delta \mathit{q}-{\displaystyle \frac{1}{2}}\mathit{q}\otimes {\widehat{\mathit{q}}}^{-1}\otimes \left\{\begin{array}{c}\widehat{\mathit{\omega }}\\ 0\end{array}\right\}={\displaystyle \frac{1}{2}}\Omega (\mathit{\omega })\delta \mathit{q}-{\displaystyle \frac{1}{2}}\delta \mathit{q}\otimes \left\{\begin{array}{c}\widehat{\mathit{\omega }}\\ 0\end{array}\right\}.$$

This allows us to write the following:

$$\delta \mathit{q}\otimes \left\{\begin{array}{c}\widehat{\mathit{\omega }}\\ 0\end{array}\right\}=\Gamma (\widehat{\mathit{\omega }})\delta \mathit{q}.$$

The quaternion error rate equation becomes the following:

$$\delta \dot{\mathit{q}}={\displaystyle \frac{1}{2}}[\Omega (\mathit{\omega })-\Gamma (\widehat{\mathit{\omega }})]\delta \mathit{q}.$$

(9)

Now, by substituting the angular velocity error given in Equation (2) into Equation (9), the bilinear differential equation for the tracking error kinematics becomes the following:

$$\delta \dot{\mathit{q}}={\displaystyle \frac{1}{2}}[\Omega (\delta \mathit{\omega }+\widehat{\mathit{\omega }})-\Gamma (\widehat{\mathit{\omega }})]\delta \mathit{q}={\displaystyle \frac{1}{2}}[\Omega (\delta \mathit{\omega })+\tilde{\Gamma }(\widehat{\mathit{\omega }})]\delta \mathit{q},$$

(10)

where $\tilde{\Gamma }(\widehat{\mathit{\omega }})$ is a matrix defined as follows:

$$\tilde{\Gamma }(\widehat{\mathit{\omega }})=\Omega (\widehat{\mathit{\omega }})-\Gamma (\widehat{\mathit{\omega }})=\left[\begin{array}{cc}-2[\widehat{\mathit{\omega }}\times ]& {\mathit{0}}_{3\times 1}\\ {\mathit{0}}_{1\times 3}& 0\end{array}\right].$$

Equation (10) can be rewritten in the following compact form:

$$\delta \dot{\mathit{q}}={\displaystyle \frac{1}{2}}\left[\begin{array}{cc}-([\delta \mathit{\omega }\times ]+2[\widehat{\mathit{\omega }}\times ])& \delta \mathit{\omega }\\ -\delta {\mathit{\omega }}^{\mathrm{T}}& 0\end{array}\right]\left\{\begin{array}{c}\delta {\mathit{q}}_v\\ \delta q_4\end{array}\right\}.$$

(11)

This equation can be split into the scalar and vector part of the quaternion as follows [28]:

$$\begin{array}{ccc}\hfill \delta {\dot{\mathit{q}}}_v& =& {\displaystyle \frac{1}{2}}\left\{-\left([\delta \mathit{\omega }\times ]+2[\widehat{\mathit{\omega }}\times ]\right)\delta {\mathit{q}}_v+\delta q_4\delta \mathit{\omega }\right\}\hfill \\ \hfill \delta {\dot{q}}_4& =& -{\displaystyle \frac{1}{2}}\delta {\mathit{\omega }}^{\mathrm{T}}\delta {\mathit{q}}_v\hfill \end{array}.$$

(12)

These above equations are exact for arbitrarily large and rapid relative rotational motions.

2.1.2. Rodrigues Parameter Error Kinematics

Rodrigues parameters are a minimum attitude parametrization. The RP vector is defined in terms of quaternion parameters as follows [10]:

$$\mathit{\rho }={\displaystyle \frac{{\mathit{q}}_v}{q_4}}=\mathit{e}tan\left({\displaystyle \frac{\varphi }{2}}\right),$$

(13)

where the inverse transformation is given as follows:

$$q_4={\displaystyle \frac{1}{\sqrt{1+{\rho }^2}}}\mathrm{and}{\mathit{q}}_v={\displaystyle \frac{\mathit{\rho }}{\sqrt{1+{\rho }^2}}},$$

where ${\rho }^2={\mathit{\rho }}^{\mathrm{T}}\mathit{\rho }$. Note that the attitude error given in Equation (12) is represented in the quaternion form. An exact nonlinear model for the quaternions with no approximation is used to preserve the quaternion unit norm. This implies that the quaternion error still represents a finite orientation that can be mapped to any other attitude representations. Here, the quaternion error to RP using Equation (13) is mapped. Thus, the RP error vector is simply expressed as follows:

$$\delta \mathit{\rho }={\displaystyle \frac{\delta {\mathit{q}}_v}{\delta q_4}}.$$

(14)

The inverse mapping for quaternion variables follows:

$$\delta q_4={\displaystyle \frac{1}{\sqrt{1+\delta {\rho }^2}}}\mathrm{and}\delta {\mathit{q}}_v={\displaystyle \frac{\delta \mathit{\rho }}{\sqrt{1+\delta {\rho }^2}}},$$

(15)

where $\delta {\rho }^2=\delta {\mathit{\rho }}^{\mathrm{T}}\delta \mathit{\rho }$. The RP error differential equation is obtained by taking the derivative of Equation (14) and substituting Equations (12) and (15), which yields the following [28]:

$$\delta \dot{\mathit{\rho }}={\displaystyle \frac{1}{2}}\left[-\left([\delta \mathit{\omega }\times ]+2[\widehat{\mathit{\omega }}\times ]\right)\delta \mathit{\rho }+\delta \mathit{\omega }\right]+{\displaystyle \frac{1}{2}}\left(\delta {\mathit{\omega }}^{\mathrm{T}}\delta \mathit{\rho }\right)\delta \mathit{\rho }.$$

(16)

Equation (16) provides the desired RP attitude error nonlinear kinematic differential equation.

2.1.3. Modified Rodrigues Parameter Error Kinematics

Modified Rodrigues parameters are an elegant addition to the family of attitude representations. MRP vector is defined in terms of the quaternion parameters by the following [10]

$$\mathit{\sigma }={\displaystyle \frac{{\mathit{q}}_v}{1+q_4}}=\mathit{e}tan\left({\displaystyle \frac{\varphi }{4}}\right).$$

(17)

The inverse transformation is given by the following:

$$q_4={\displaystyle \frac{1-{\sigma }^2}{1+{\sigma }^2}}\mathrm{and}{\mathit{q}}_v={\displaystyle \frac{2\mathit{\sigma }}{1+{\sigma }^2}}.$$

Similarly, since the attitude error in Equation (12) is expressed by quaternion, the mapping into MRP can be performed using Equation (17). Thus, MRP error vector is expressed as follows:

$$\delta \mathit{\sigma }={\displaystyle \frac{\delta {\mathit{q}}_v}{1+\delta q_4}}.$$

(18)

The inverse mapping for quaternion variables follows:

$$\delta q_4={\displaystyle \frac{1-\delta {\sigma }^2}{1+\delta {\sigma }^2}}\mathrm{and}\delta {\mathit{q}}_v={\displaystyle \frac{2\delta \mathit{\sigma }}{1+\delta {\sigma }^2}}.$$

(19)

The MRP error differential equation is obtained by taking the derivative of Equation (18) and substituting Equation (12) and Equation (19), which yields the following compact, nonlinear, third-order form [28]:

$$\delta \dot{\mathit{\sigma }}={\displaystyle \frac{1}{4}}\left[-2\left([\delta \mathit{\omega }\times ]+2[\widehat{\mathit{\omega }}\times ]\right)\delta \mathit{\sigma }+(1-\delta {\sigma }^2)\delta \mathit{\omega }\right]+{\displaystyle \frac{1}{2}}\left(\delta {\mathit{\omega }}^{\mathrm{T}}\delta \mathit{\sigma }\right)\delta \mathit{\sigma }.$$

(20)

Equation (20) provides the exact MRP attitude error kinematic differential equation.

2.1.4. Euler Angles Error Kinematics

The most famous attitude representation is described by three angles, known as Euler angles, $({\theta }_1,{\theta }_2,{\theta }_3)$ associated with subsequent rotations about three coordinate axes. The variations of these angles represent the attitude error of $\delta C$. There are several conventions for Euler angles, depending on the axes about which the rotations are carried out. In the following, we assume the rotation is 3-1-3 (yaw-roll-yaw) sequences, then we generalize the expression for arbitrary rotation sequence. We start from the mapping equations from quaternion to Euler angles [6]. For the 3-1-3 set, the transformation is as follows:

$$\begin{array}{c}\hfill \left\{\begin{array}{c}\delta {\mathit{q}}_v\\ \delta q_4\end{array}\right\}={\left\{\begin{array}{c}sin(\frac{\delta {\theta }_2}{2})cos(\frac{\delta {\theta }_1-\delta {\theta }_3}{2})\\ sin(\frac{\delta {\theta }_2}{2})sin(\frac{\delta {\theta }_1-\delta {\theta }_3}{2})\\ cos(\frac{\delta {\theta }_2}{2})sin(\frac{\delta {\theta }_1+\delta {\theta }_3}{2})\\ cos(\frac{\delta {\theta }_2}{2})cos(\frac{\delta {\theta }_1+\delta {\theta }_3}{2})\end{array}\right\}}_{313}={\Theta }_{313}(\delta {\theta }_1,\delta {\theta }_2,\delta {\theta }_3).\end{array}$$

(21)

Differentiating Equation (21), we obtain the following:

$$\begin{array}{c}\hfill \left\{\begin{array}{c}\delta \dot{{\mathit{q}}_v}\\ \delta \dot{q}4\end{array}\right\}=H_{313}(\delta {\theta }_1,\delta {\theta }_2,\delta {\theta }_3){\left\{\begin{array}{c}\delta {\dot{\theta }}_1\\ \delta {\dot{\theta }}_2\\ \delta {\dot{\theta }}_3\end{array}\right\}}_{313},\end{array}$$

where $H_{313}(\delta {\theta }_1,\delta {\theta }_2,\delta {\theta }_3)$ is a $4\times 3$ matrix. Thus, Euler angle rates can be written in the following least-squares solution:

$$\begin{array}{c}\hfill {\left\{\begin{array}{c}\delta {\dot{\theta }}_1\\ \delta {\dot{\theta }}_2\\ \delta {\dot{\theta }}_3\end{array}\right\}}_{313}={(H_{313}^{\mathrm{T}}{H}_{313})}^{-1}{H}_{313}^{\mathrm{T}}\left\{\begin{array}{c}\delta \dot{{\mathit{q}}_v}\\ \delta {\dot{q}}_4\end{array}\right\}.\end{array}$$

Substituting Equation (12) and making use of Equation (21), we obtain the following:

$$\begin{array}{cc}\hfill {\left\{\begin{array}{c}\delta {\dot{\theta }}_1\\ \delta {\dot{\theta }}_2\\ \delta {\dot{\theta }}_3\end{array}\right\}}_{313}=& \frac{1}{2}{(H_{313}^{\mathrm{T}}{H}_{313})}^{-1}{H}_{313}^{\mathrm{T}}\left[\begin{array}{cc}-([\delta \mathit{\omega }\times ]+2[\widehat{\mathit{\omega }}\times ])& \delta \mathit{\omega }\\ -\delta {\mathit{\omega }}^{\mathrm{T}}& 0\end{array}\right]{\Theta }_{313}(\delta {\theta }_1,\delta {\theta }_2,\delta {\theta }_3).\end{array}$$

In general, for the generic i-j-k Euler angles sequence, the formula is as follows [28]:

$$\begin{array}{cc}\hfill {\left\{\begin{array}{c}\delta {\dot{\theta }}_i\\ \delta {\dot{\theta }}_j\\ \delta {\dot{\theta }}_k\end{array}\right\}}_{ijk}=& \frac{1}{2}{(H_{ijk}^{\mathrm{T}}{H}_{ijk})}^{-1}{H}_{ijk}^{\mathrm{T}}\left[\begin{array}{cc}-([\delta \mathit{\omega }\times ]+2[\widehat{\mathit{\omega }}\times ])& \delta \mathit{\omega }\\ -\delta {\mathit{\omega }}^{\mathrm{T}}& 0\end{array}\right]{\Theta }_{ijk}(\delta {\theta }_1,\delta {\theta }_2,\delta {\theta }_3).\end{array}$$

(22)

Equation (22) represents the attitude error kinematic equation using any Euler angle sequence.

2.1.5. Principal Axis and Angle Error Kinematics

Any rigid-body rotation can be obtained by a single rotation about a principal axis, $\mathit{e}$, by a principal angle, $\varphi$. To derive the kinematics of the principal axis/angle for attitude error, we start from the definition of the quaternion:

$$\delta q_4=cos(\delta \varphi /2)\mathrm{and}\delta {\mathit{q}}_v=\delta \mathit{e}sin(\delta \varphi /2).$$

(23)

Taking the derivative of Equation (23), substituting Equation (12), and solving for $\delta \dot{\varphi }$ and $\delta \dot{\mathit{e}}$, we obtain the following [28]:

$$\delta \dot{\varphi }=\delta {\mathit{\omega }}^{\mathrm{T}}\delta \widehat{\mathit{e}},$$

(24)

where $\delta \widehat{\mathit{e}}=\frac{\delta \mathit{e}}{\sqrt{\delta {\mathit{e}}^{\mathrm{T}}\delta \mathit{e}}}$ and

$$\delta \dot{\mathit{e}}=-\frac{1}{2}\left(\left([\delta \mathit{\omega }\times ]+2[\widehat{\mathit{\omega }}\times ]\right)\delta \widehat{\mathit{e}}+\left[(\delta {\mathit{\omega }}^{\mathrm{T}}\delta \widehat{\mathit{e}})\delta \widehat{\mathit{e}}-\delta \mathit{\omega }\right]cot(\delta \varphi /2)\right).$$

(25)

Equations (24) and (25) are the desired nonlinear kinematic differential equation of the attitude error using principal axis and principal angle.

2.1.6. Direction Cosine Matrix Error Kinematics

The DCM error can be written as follows:

$$\delta C=C{\widehat{C}}^{\mathrm{T}}.$$

(26)

Performing the derivative of Equation (26) and making use of the attitude kinematics, $\dot{C}=-[\mathit{\omega }\times ]C$, and the DCM inverse identity, ${\dot{\widehat{C}}}^{\mathrm{T}}=-{\widehat{C}}^{\mathrm{T}}\dot{\widehat{C}}{\widehat{C}}^{\mathrm{T}}$, we obtain the following [28]:

$$\delta \dot{C}=-[\delta \mathit{\omega }\times ]\delta C-[\widehat{\mathit{\omega }}\times ]\delta C+\delta C[\widehat{\mathit{\omega }}\times ].$$

(27)

2.1.7. Cayley–Klein Error Parameters Kinematics

Cayley–Klein parameters is an attitude representation provided by a $2\times 2$ complex matrix. This matrix is as follows:

$$\delta K=\delta q_4{I}_{2\times 2}+i\left(\delta q_1{\sigma }_1+\delta q_2{\sigma }_2+\delta q_3{\sigma }_3\right)=\left[\begin{array}{cc}\delta q_4+i\delta q_3& \delta q_2+i\delta q_1\\ -\delta q_2+i\delta q_1& \delta q_4-i\delta q_3\end{array}\right],$$

(28)

where ${\sigma }_1=\left[\begin{array}{cc}0& 1\\ 1& 0\end{array}\right]$, ${\sigma }_2=\left[\begin{array}{cc}0& i\\ -i& 0\end{array}\right]$, and ${\sigma }_3=\left[\begin{array}{cc}1& 0\\ 0& -1\end{array}\right]$ are the three Pauli spin matrices. The principal angle can be computed from the following:

$$\delta \varphi =2{cos}^{-1}\left[\frac{1}{2}\mathrm{tr}\left(\delta K\right)\right].$$

Rewriting Equation (28) in column form, we obtain the following:

$$\begin{array}{c}\hfill \mathrm{col}(\delta K)=\left\{\begin{array}{c}\delta K_{1,1}\\ \delta K_{2,1}\\ \delta K_{1,2}\\ \delta K_{2,2}\end{array}\right\}=\left\{\begin{array}{c}\delta q_4+i\delta q_3\\ -\delta q_2+i\delta q_1\\ \delta q_2+i\delta q_1\\ \delta q_4-i\delta q_3\end{array}\right\}=\left[\begin{array}{cccc}0& 0& i& 1\\ i& -1& 0& 0\\ i& 1& 0& 0\\ 0& 0& -i& 1\end{array}\right]\left\{\begin{array}{c}\delta {\mathit{q}}_v\\ \delta q_4\end{array}\right\}={\Psi }_0\delta \mathit{q},\end{array}$$

(29)

where ${\Psi }_0$ is a non-singular constant matrix. Differentiating Equation (29), substituting Equation (12), and using ${\mathit{q}}_v={\Psi }_0^{-1}\mathrm{col}(\delta K)$, we obtain the following [28]:

$$\mathrm{col}(\delta \dot{K})={\displaystyle \frac{{\Psi }_0}{2}}\left[\begin{array}{cc}-[\delta \mathit{\omega }\times ]+2[\widehat{\mathit{\omega }}\times ]& \delta \mathit{\omega }\\ -\delta {\mathit{\omega }}^{\mathrm{T}}& 0\end{array}\right]{\Psi }_0^{-1}\mathrm{col}(\delta K),$$

(30)

where the $\mathrm{col}(\delta K)$ has to satisfy the constraint equation $\delta {\mathit{q}}^{\mathrm{T}}\delta \mathit{q}=1$ that leads to $\mathrm{col}{(\delta K)}^{\mathrm{T}}{\Psi }_0^{-T}{\Psi }_0^{-1}\mathrm{col}(\delta K)=1$ or $\mathrm{col}{(\delta K)}^{\mathrm{T}}\mathrm{col}(\delta K)=2$.

2.2. Estimation/Control Case: Angular Velocity Estimated in the Estimated Attitude Frame

In this section, the estimated and the true angular velocities are expressed in different coordinate frames: $\delta \mathit{\omega }=\mathit{\omega }-\delta C\widehat{\mathit{\omega }}$.

This equation explicitly accounts for the obvious truth that the estimated and true angular velocity vectors are expressed in different coordinate frames. This definition explicitly computes the angular velocity error in the current body frame. This kinematic variable definition leads to the following angular velocity error dynamics equation:

$$\begin{array}{cc}\hfill \delta \dot{\mathit{\omega }}=& -I^{-1}\left(\left[\delta C\widehat{\mathit{\omega }}\times \right]I-[I\delta C\widehat{\mathit{\omega }}\times ]\right)\delta \mathit{\omega }-I^{-1}[\delta \mathit{\omega }\times ]I\delta \mathit{\omega }+I^{-1}\mathit{u}+\hfill \\ \hfill & +[\delta \mathit{\omega }\times ]\delta C\widehat{\mathit{\omega }}-\dot{\widehat{\mathit{\omega }}}-I^{-1}\left[\delta C\widehat{\mathit{\omega }}\times \right]I\delta C\widehat{\mathit{\omega }}.\hfill \end{array}$$

(31)

It is clear that the angular velocity error dynamics equation is coupled with the attitude solution. This is expected as the estimated angular velocity has to be mapped to the true frame using the attitude corrective matrix, $\delta C$.

2.2.1. Direction Cosine Matrix Error Kinematics

To investigate the new attitude error parametrization for the estimation/control case, the direction cosine matrix is differentiated for the attitude error given in Equation (1):

$$\delta C=C{\widehat{C}}^{\mathrm{T}}\to \delta \dot{C}=\dot{C}{\widehat{C}}^{\mathrm{T}}+C{\dot{\widehat{C}}}^{\mathrm{T}}.$$

(32)

Recall the attitude kinematics of the current motion $\dot{C}=-[\mathit{\omega }\times ]C$ and the estimated motion $\dot{\widehat{C}}=-[\widehat{\mathit{\omega }}\times ]\widehat{C}$. Equation (32) can be written:

$$\delta \dot{C}=-[\mathit{\omega }\times ]\delta C+\delta C[\widehat{\mathit{\omega }}\times ].$$

(33)

The angular velocity error is $\delta \mathit{\omega }=\mathit{\omega }-\delta C\widehat{\mathit{\omega }}$. Therefore, we obtain the following:

$$\delta \dot{C}=-[\delta \mathit{\omega }\times ]\delta C-[\delta C\widehat{\mathit{\omega }}\times ]\delta C+\delta C[\widehat{\mathit{\omega }}\times ].$$

(34)

Using the transformation of skew-symmetric tensor identity [4], we obtain the following:

$$[\delta C\widehat{\mathit{\omega }}\times ]=\delta C[\widehat{\mathit{\omega }}\times ]\delta C^{\mathrm{T}}.$$

(35)

Also, since $\delta C$ is an orthogonal matrix, Equation (34) reduces to the following:

$$\delta \dot{C}=-[\delta \mathit{\omega }\times ]\delta C.$$

(36)

It is interesting that the attitude error kinematics equation is similar to the attitude kinematics equation. This result agrees with the demonstration presented in Tanygin’s work [24]. Hence, the attitude error kinematics using other attitude parameterizations can be simply obtained.

2.2.2. Quaternion Error Kinematics

The quaternion error is a four-dimensional vector defined as follows:

$$\delta \mathit{q}=\left\{\begin{array}{c}\delta {\mathit{q}}_v\\ \delta q_4\end{array}\right\}=\left\{\begin{array}{c}\delta q_1\\ \delta q_2\\ \delta q_3\\ \delta q_4\end{array}\right\}.$$

The transformation from quaternion error, $\delta \mathit{q}$, to DCM error, $\delta C$, is given by the following:

$$\delta C=\left[\begin{array}{c}(\delta q_4^2-\delta {\mathit{q}}_v^{\mathrm{T}}\delta {\mathit{q}}_v)I+2\delta {\mathit{q}}_v\delta {\mathit{q}}_v^{\mathrm{T}}-2\delta q_4[\delta {\mathit{q}}_v\times ]\end{array}\right].$$

(37)

The inverse transformations is as follows:

$$\begin{array}{c}\hfill \delta q_4=\pm \frac{1}{2}\sqrt{1+\mathrm{tr}\left[\delta C\right]}\end{array}$$

(38)

$$\begin{array}{c}\hfill \delta {\mathit{q}}_v=\frac{1}{4\delta q_4}\left\{\begin{array}{c}\delta C_{23}-\delta C_{32}\\ \delta C_{31}-\delta C_{13}\\ \delta C_{12}-\delta C_{21}\end{array}\right\}.\end{array}$$

(39)

This transformation shows the redundancy in selecting the sign of the scalar parameter, $\delta q_4$. The positive sign is associated to $\delta \varphi \le \pi$, while the negative is associated to $\pi <\delta \varphi \le 2\pi$.

The kinematics differential equation of the quaternion error is derived by differentiating Equation (38):

$$\begin{array}{c}\hfill \delta {\dot{q}}_4=\frac{\delta {\dot{C}}_{11}+\delta {\dot{C}}_{22}+\delta {\dot{C}}_{33}}{8\delta q_4}.\end{array}$$

(40)

Substituting the expressions of the diagonal elements

$$\begin{array}{cc}\hfill \delta {\dot{C}}_{11}=& -(\delta C_{23}-\delta C_{32})\delta {\omega }_1\hfill \\ \hfill \delta {\dot{C}}_{22}=& -(\delta C_{31}-\delta C_{13})\delta {\omega }_2\hfill \\ \hfill \delta {\dot{C}}_{33}=& -(\delta C_{12}-\delta C_{21})\delta {\omega }_3\hfill \end{array}$$

in Equation (40) yields the following:

$$\delta {\dot{q}}_4=\frac{1}{2}(-\delta q_1\delta {\omega }_1-\delta q_2\delta {\omega }_2-\delta q_3\delta {\omega }_3)=-\frac{1}{2}\delta {\mathit{q}}_v^{\mathrm{T}}\delta \mathit{\omega }.$$

(41)

Similar derivations are applied on $\delta {\dot{q}}_1$, $\delta {\dot{q}}_2$, and $\delta {\dot{q}}_3$ in Equation (). The kinematic error differential equation for quaternion is then the following:

$$\delta \dot{\mathit{q}}={\displaystyle \frac{1}{2}}\left[\begin{array}{c}\delta q_4{I}_{3\times 3}+[\mathit{\delta }{\mathit{q}}_v\times ]\\ -\delta {\mathit{q}}_v^{\mathrm{T}}\end{array}\right]\delta \mathit{\omega }={\displaystyle \frac{1}{2}}\left[\begin{array}{cc}-[\mathit{\delta }\mathit{\omega }\times ]& \delta \mathit{\omega }\\ -\delta {\mathit{\omega }}^{\mathrm{T}}& 0\end{array}\right]\delta \mathit{q},$$

(42)

where $I_{3\times 3}$ is the $3\times 3$ identity matrix.

2.2.3. Rodrigues Parameter Error Kinematics

Rodrigues parameters error vector is simply expressed by the following:

$$\delta \mathit{\rho }={\displaystyle \frac{\delta {\mathit{q}}_v}{\delta q_4}}=tan\left(\frac{\delta \varphi }{2}\right)\delta \mathit{e},$$

(43)

Also, the inverse mapping for quaternion variables follow:

$$\delta q_4={\displaystyle \frac{1}{\sqrt{1+\delta {\rho }^2}}}\mathrm{and}\delta {\mathit{q}}_v={\displaystyle \frac{\delta \mathit{\rho }}{\sqrt{1+\delta {\rho }^2}}},$$

(44)

where $\delta {\rho }^2=\delta {\mathit{\rho }}^{\mathrm{T}}\delta \mathit{\rho }$. The differential equation for the RP error follows by taking the derivative of Equation (43):

$$\delta \dot{\mathit{\rho }}={\displaystyle \frac{\delta \dot{{\mathit{q}}_v}}{\delta q_4}}-{\displaystyle \frac{\delta {\dot{q}}_4\delta {\mathit{q}}_v}{{\left(\delta q_4\right)}^2}}.$$

(45)

Substituting Equation (42) into Equation (45) yields the following:

$$\delta \dot{\mathit{\rho }}={\displaystyle \frac{1}{2}}\left[{\displaystyle \frac{\delta q_4{I}_{3\times 3}+[\mathit{\delta }{\mathit{q}}_v\times ]}{\delta q_4}}+{\displaystyle \frac{\delta {\mathit{q}}_v}{{\left(\delta q_4\right)}^2}}\delta {\mathit{q}}_v^{\mathrm{T}}\right]\delta \mathit{\omega }.$$

This equation can be further simplified by substituting Equation (44) to yield the compact quadratic form:

$$\delta \dot{\mathit{\rho }}={\displaystyle \frac{1}{2}}\left[I_{3\times 3}+[\mathit{\delta }\mathit{\rho }\times ]+\delta \mathit{\rho }\delta {\mathit{\rho }}^{\mathrm{T}}\right]\delta \mathit{\omega }.$$

(46)

Equation (46) is the RP attitude error kinematic differential equation. The above form approaches singularity when $\left|\delta \varphi \right|\to \pm \pi$.

2.2.4. Modified Rodrigues Parameter Error Kinematics

Modified Rodrigues parameters error vector is expressed as follows:

$$\delta \mathit{\sigma }={\displaystyle \frac{\delta {\mathit{q}}_v}{1+\delta q_4}}=tan\left(\frac{\delta \varphi }{4}\right)\delta \mathit{e},$$

(47)

The inverse mapping to quaternion is as follows:

$$\delta q_4={\displaystyle \frac{1-\delta {\sigma }^2}{1+\delta {\sigma }^2}}\mathrm{and}\delta {\mathit{q}}_v={\displaystyle \frac{2\delta \mathit{\sigma }}{1+\delta {\sigma }^2}}.$$

(48)

The differential equation for the MRP error follows from the derivative of Equation (47):

$$\delta \dot{\mathit{\sigma }}={\displaystyle \frac{\delta \dot{{\mathit{q}}_v}}{1+\delta q_4}}-{\displaystyle \frac{\delta {\dot{q}}_4\delta {\mathit{q}}_v}{{(1+\delta q_4)}^2}}.$$

(49)

Substituting Equation (42) into Equation (49) yields the following:

$$\delta \dot{\mathit{\sigma }}={\displaystyle \frac{1}{2}}\left[{\displaystyle \frac{\delta q_4{I}_{3\times 3}+[\mathit{\delta }{\mathit{q}}_v\times ]}{1+\delta q_4}}+{\displaystyle \frac{\delta {\mathit{q}}_v\delta {\mathit{q}}_v^{\mathrm{T}}}{{(1+\delta q_4)}^2}}\right]\delta \mathit{\omega }.$$

This equation is simplified by substituting Equation (48) to obtain a compact quadratic form:

$$\delta \dot{\mathit{\sigma }}={\displaystyle \frac{1}{4}}\left[(1-\delta {\sigma }^2)I_{3\times 3}+2[\delta \mathit{\sigma }\times ]+2\delta {\mathit{\sigma }}^{\mathrm{T}}\delta \mathit{\sigma }\right]\delta \mathit{\omega }.$$

(50)

Equation (50) provides the MRP attitude error kinematic differential equation. The above form approaches singular behavior as $\left|\delta \varphi \right|\to \pm 2\pi$.

2.2.5. Euler Angles Error Kinematics

There are 12 distinct Euler angles sequences, depending on the axes about which the rotations are carried out. Review the development in Section 2.1.4; similar derivations are followed here that yield to the general i-j-k Euler angle error kinematics equations:

$$\begin{array}{cc}\hfill {\left\{\begin{array}{c}\delta {\dot{\theta }}_i\\ \delta {\dot{\theta }}_j\\ \delta {\dot{\theta }}_k\end{array}\right\}}_{ijk}=& \frac{1}{2}{(H_{ijk}^{\mathrm{T}}{H}_{ijk})}^{-1}{H}_{ijk}^{\mathrm{T}}\left[\begin{array}{cc}-[\delta \mathit{\omega }\times ]& \delta \mathit{\omega }\\ -\delta {\mathit{\omega }}^{\mathrm{T}}& 0\end{array}\right]{\Theta }_{ijk}(\delta {\theta }_1,\delta {\theta }_2,\delta {\theta }_3).\end{array}$$

(51)

Equation (51) is the desired kinematic differential equation using Euler angles to represent the attitude error.

Alternatively, the Euler angles error kinematics can be derived by writing the angular velocity error vector in the body frame in terms of the Euler angles error rates. For example, the angular velocity error vector for 3-1-3 rotation sequence is given by the following:

$$\delta \mathit{\omega }=R_{313}(\delta {\theta }_1,\delta {\theta }_2,\delta {\theta }_3)\left\{\begin{array}{c}0\\ 0\\ \delta {\dot{\theta }}_1\end{array}\right\}+R_{31}(\delta {\theta }_2,\delta {\theta }_3)\left\{\begin{array}{c}\delta {\dot{\theta }}_2\\ 0\\ 0\end{array}\right\}+R_3(\delta {\theta }_3)\left\{\begin{array}{c}0\\ 0\\ \delta {\dot{\theta }}_3\end{array}\right\},$$

(52)

where $R_{313}(\delta {\theta }_1,\delta {\theta }_2,\delta {\theta }_3)=R_3(\delta {\theta }_3)R_1(\delta {\theta }_2)R_3(\delta {\theta }_1)$ is the 3-1-3 rotation sequence, where

$$\begin{array}{cc}\hfill R_1(\delta \theta )=& \left[\begin{array}{ccc}1& 0& 0\\ 0& cos(\delta \theta )& sin(\delta \theta )\\ 0& -sin(\delta \theta )& cos(\delta \theta )\end{array}\right],\hfill \\ \hfill R_2(\delta \theta )=& \left[\begin{array}{ccc}cos(\delta \theta )& 0& -sin(\delta \theta )\\ 0& 1& 0\\ sin(\delta \theta )& 0& cos(\delta \theta )\end{array}\right],\mathrm{and}\hfill \\ \hfill R_3(\delta \theta )=& \left[\begin{array}{ccc}cos(\delta \theta )& -sin(\delta \theta )& 0\\ sin(\delta \theta )& cos(\delta \theta )& 0\\ 0& 0& 1\end{array}\right].\hfill \end{array}$$

(53)

Equation (52) can be written in the compact form:

$$\delta \mathit{\omega }={\mathcal{M}}_{313}\left\{\begin{array}{c}\delta {\dot{\theta }}_1\\ \delta {\dot{\theta }}_2\\ \delta {\dot{\theta }}_3\end{array}\right\},$$

(54)

where

$${\mathcal{M}}_{313}=\left[\begin{array}{ccc}sin(\delta {\theta }_2)sin(\delta {\theta }_3)& cos(\delta {\theta }_3)& 0\\ sin(\delta {\theta }_2)cos(\delta {\theta }_3)& -sin(\delta {\theta }_3)& 0\\ cos(\delta {\theta }_2)& 0& 1\end{array}\right].$$

The kinematic differential equation of 3-1-3 Euler angles error is the inverse of Equation (54):

$$\left\{\begin{array}{c}\delta {\dot{\theta }}_1\\ \delta {\dot{\theta }}_2\\ \delta {\dot{\theta }}_3\end{array}\right\}={\mathcal{M}}_{313}^{-1}\left\{\begin{array}{c}\delta {\omega }_1\\ \delta {\omega }_2\\ \delta {\omega }_3\end{array}\right\}={\mathcal{M}}_{313}^{-1}\delta \mathit{\omega }.$$

(55)

The complete set of 12 Euler angles error kinematics in terms of ${\mathcal{M}}_{ijk}^{-1}$ and the angular velocity error vector is provided in Appendix A.

2.2.6. Principal Axis and Angle Error Kinematics

To derive the kinematics of the principal axis/angle for attitude error, we start from the definition of the quaternion:

$$\delta q_4=cos(\delta \varphi /2)\mathrm{and}\delta {\mathit{q}}_v=\delta \mathit{e}sin(\delta \varphi /2).$$

Taking the derivative and solving for $\delta \dot{\varphi }$ and $\delta \dot{\mathit{e}}$, we obtain the following:

$$\delta \dot{\varphi }=-\frac{2\delta {\dot{q}}_4}{sin(\delta \varphi /2)}\mathrm{and}\delta \dot{\mathit{e}}=\frac{\delta {\dot{\mathit{q}}}_v-\frac{1}{2}\delta \mathit{e}\delta \dot{\varphi }cos(\delta \varphi /2)}{sin(\delta \varphi /2)}.$$

Substituting Equation (42) and making use of $\delta {\mathit{q}}_v=\delta \widehat{\mathit{e}}sin\left(\delta \varphi /2\right)$ and the identity $[\delta \widehat{\mathit{e}}\times ][\delta \widehat{\mathit{e}}\times ]=\delta \widehat{\mathit{e}}\delta {\widehat{\mathit{e}}}^{\mathrm{T}}-I_{3\times 3}$, we obtain the following:

$$\delta \dot{\varphi }=\delta {\mathit{\omega }}^{\mathrm{T}}\delta \widehat{\mathit{e}},$$

(56)

where $\delta \widehat{\mathit{e}}={\displaystyle \frac{\delta \mathit{e}}{\sqrt{\delta {\mathit{e}}^{\mathrm{T}}\delta \mathit{e}}}}$ and

$$\delta \dot{\mathit{e}}={\displaystyle \frac{1}{2}}\left\{[\delta \widehat{\mathit{e}}\times ]-[\delta \widehat{\mathit{e}}\times ][\delta \widehat{\mathit{e}}\times ]cot(\delta \varphi /2)\right\}\delta \mathit{\omega }.$$

(57)

Equations (56) and (57) represent the kinematic equations of the attitude error using principal axis and principal angle.

2.2.7. Cayley–Klein Error Parameters Kinematics

Cayley–Klein parameters is an attitude representation provided by a $2\times 2$ complex matrix. This matrix is as follows:

$$\delta K=\delta q_4{I}_{2\times 2}+i\left(\delta q_1{\sigma }_1+\delta q_2{\sigma }_2+\delta q_3{\sigma }_3\right)=\left[\begin{array}{cc}\delta q_4+i\delta q_3& \delta q_2+i\delta q_1\\ -\delta q_2+i\delta q_1& \delta q_4-i\delta q_3\end{array}\right],$$

where ${\sigma }_1$, ${\sigma }_2$, and ${\sigma }_3$ are the three Pauli spin matrices. The principal angle can be computed from the following:

$$\delta \varphi =2{cos}^{-1}\left[\frac{1}{2}\mathrm{tr}\left(\delta K\right)\right].$$

It has been already shown in (Section 2.1.7) that the Cayley–Klein parameters can be written in the column form:

$$\begin{array}{c}\hfill \mathrm{col}(\delta K)=\left\{\begin{array}{c}\delta K_{1,1}\\ \delta K_{2,1}\\ \delta K_{1,2}\\ \delta K_{2,2}\end{array}\right\}={\Psi }_0\delta \mathit{q},\end{array}$$

where ${\Psi }_0=\left[\begin{array}{cccc}0& 0& i& 1\\ i& -1& 0& 0\\ i& 1& 0& 0\\ 0& 0& -i& 1\end{array}\right]$ is a non-singular constant matrix. The kinematics equation of the Cayley–Klein error column vector is as follows:

$$\begin{array}{c}\hfill \mathrm{col}(\delta \dot{K})={\Psi }_0\delta \dot{\mathit{q}}.\end{array}$$

(58)

Substituting Equation (42) into Equation (58) and making use of $\mathrm{col}(\delta K)={\Psi }_0\delta \mathit{q}$, the kinematics equation of the Cayley–Klein error column vector yields the following:

$$\mathrm{col}(\delta \dot{K})={\displaystyle \frac{1}{2}}{\Psi }_0\left[\begin{array}{cc}-[\delta \mathit{\omega }\times ]& \delta \mathit{\omega }\\ -\delta {\mathit{\omega }}^{\mathrm{T}}& 0\end{array}\right]{\Psi }_0^{-1}\mathrm{col}(\delta K),$$

(59)

where the $\mathrm{col}(\delta K)$ has to satisfy the constraint equation $\mathrm{col}{(\delta K)}^{\mathrm{T}}\mathrm{col}(\delta K)=2$.

Appendix B summarizes the attitude error kinematics using the two definitions of the angular velocity error.

2.3. Numerical Validation

Numerical integrations of all the kinematic equations presented are performed for arbitrary initial conditions to validate the machine error accuracy for each attitude representation. For this particular example, the initial conditions are given in

Table 1. Initial conditions.

Parameter	Initial Condition
$\mathit{\omega }$ (rad/s)	${\{0.7972,0.5202,0.3064\}}^{\mathrm{T}}$
$\widehat{\mathit{\omega }}$ (rad/s)	${\{0.7931,0.5898,0.1521\}}^{\mathrm{T}}$
$\mathit{q}$	${\{-0.3112,-0.2937,0.5374,-0.7267\}}^{\mathrm{T}}$
$\widehat{\mathit{q}}$	${\{-0.0104,-0.8248,0.4319,0.3647\}}^{\mathrm{T}}$

, where constant angular velocity is considered. The validation test is performed as follows:

• Integrate the true attitude kinematics for each attitude representation using the attitude kinematics equations;
• Transform the time history solution of the true attitude for each attitude representation to the direction cosine matrix C or quaternion $\mathit{q}$;
• Integrate the estimated attitude kinematics for each attitude representation using the attitude kinematics equations;
• Transform the time history solution of the estimated attitude for each attitude representation to the direction cosine matrix $\widehat{C}$ or quaternion $\widehat{\mathit{q}}$;
• Calculate the attitude error of the two solutions in steps 2 and 4 using $\delta C=C{\widehat{C}}^{\mathrm{T}}$ or $\delta \mathit{q}=\mathit{q}\otimes {\widehat{\mathit{q}}}^{-1}$; then, calculate the principal angle of the attitude error, $\delta {\varphi }^{*}$;
• For approaches 1 and 2, integrate the attitude error kinematics for each attitude representation using the attitude error kinematics equations in Appendix B (Table Figure A1); then, calculate the principal angle of the attitude error $\delta \varphi$ for each approach;
• Calculate the absolute error $\Delta (\delta \varphi )=|\delta {\varphi }^{*}-\delta \varphi |$, as shown in Figure 2.

Figure 2 represents the validation test of the two approaches (step 6) compared to the classical approach (step 5). Figure 2a,b show the computation error, $\Delta (\delta \varphi )=|\delta {\varphi }^{*}-\delta \varphi |$, of the attitude error computed using the first approach (Figure 2a) and the second approach (Figure 2b) for the following attitude representations: direction cosine matrix, quaternion, Rodrigues parameters, and modified Rodrigues parameters. The first approach represents the simulation case, denoted by kinematic error 1. The second approach represents theestimation/control case, denoted by kinematic error 2. The two figures Figure 2c,d show the computation error, $\Delta (\delta \varphi )=|\delta {\varphi }^{*}-\delta \varphi |$, of the attitude error computed using the first approach (Figure 2c) and the second approach (Figure 2d) for the following attitude representations: principal axis/angle, Euler angles (3-1-3), Euler angles (1-2-3), and Cayley–Klein parameters. The computation error retains the accuracy level of the numerical integrator used (MATLAB®  function ode45).

3. Kalman Filter

In this section, a sequential extended Kalman filter (EKF) formulation is developed and presented for the two approaches. The development of compact forms of nonlinear attitude error kinematics enables the applications of arbitrarily large relative rotations and rotation rates in different attitude coordinates. Since quaternions present no singularities, it is the most popular coordinate for attitude estimation. However, it must obey the norm constraint.

3.1. Estimated Angular Velocity Defined in the True Attitude Frame

The estimated angular velocity vector $\widehat{\mathit{\omega }}$ is defined in the true body axes frame ($\mathit{x},\mathit{y},\mathit{z}$). In this case, $\mathit{\omega }$ and $\widehat{\mathit{\omega }}$ are defined in the same reference frame. Therefore, the angular velocity error vector is as follows:

$$\delta \mathit{\omega }=\mathit{\omega }-\widehat{\mathit{\omega }}.$$

This case finds application during simulations when the true attitude is available. The quaternion error kinematics is given by Equation (12), which can be linearized for first-order approximation to the following [8]:

$$\begin{array}{ccc}\hfill \delta {\dot{\mathit{q}}}_v& \approx & -[\widehat{\mathit{\omega }}\times ]\delta {\mathit{q}}_v+{\displaystyle \frac{1}{2}}\delta \mathit{\omega }\hfill \\ \hfill \delta {\dot{q}}_4& \approx & 0.\hfill \end{array}$$

(60)

Unfortunately, this linearization requires that the estimated quaternion is close to the true quaternion to preserve the normalization constraint.

A common sensor model to measure the angular rate is the rate-integrating gyro, which is defined by the following:

$$\begin{array}{ccc}\hfill \mathit{\omega }& =& \tilde{\mathit{\omega }}-\mathit{\beta }-{\mathit{\eta }}_v\hfill \\ \hfill \dot{\mathit{\beta }}& =& {\mathit{\eta }}_u,\hfill \end{array}$$

(61)

where ${\mathit{\eta }}_v\sim \mathcal{N}(0,{\sigma }_v^2{I}_{3\times 3})$ and ${\mathit{\eta }}_u\sim \mathcal{N}(0,{\sigma }_u^2{I}_{3\times 3})$ are zero-mean Gaussian noise with variances given by ${\sigma }_v^2$ and ${\sigma }_u^2$, respectively; $\mathit{\beta }$ is the gyro bias vector; and $\tilde{\mathit{\omega }}$ is the measured angular velocity, which is given by the following:

$$\tilde{\mathit{\omega }}=\widehat{\mathit{\omega }}+\widehat{\mathit{\beta }},$$

(62)

where $\widehat{\mathit{\omega }}$ and $\widehat{\mathit{\beta }}$ are the estimated angular velocity and the estimated gyro bias vectors, respectively. Hence, the angular velocity error is given by the following:

$$\delta \mathit{\omega }=-(\delta \mathit{\beta }+{\mathit{\eta }}_v),$$

(63)

where $\delta \mathit{\beta }=\mathit{\beta }-\widehat{\mathit{\beta }}$. The multiplicative extended Kalman filter error model is now given by the following (note: $\dot{\widehat{\mathit{\beta }}}=0$):

$$\begin{array}{ccc}\hfill \delta {\dot{\mathit{q}}}_v& \approx & -[\widehat{\mathit{\omega }}\times ]\delta {\mathit{q}}_v-{\displaystyle \frac{1}{2}}(\delta \mathit{\beta }+{\mathit{\eta }}_v)\hfill \\ \hfill \delta \dot{\mathit{\beta }}& \approx & {\mathit{\eta }}_u.\hfill \end{array}$$

(64)

3.2. Estimated Angular Velocity Defined in the Estimated Attitude Frame

In this section, the estimated and the true angular velocities are expressed in different coordinate frames, $\delta \mathit{\omega }=\mathit{\omega }-\delta C\widehat{\mathit{\omega }}$. It has been discussed earlier that the attitude error kinematics equations for this approach take the same form as the attitude kinematics equations; see Figure A1. The quaternion error kinematics is linearized for first-order approximation to the following:

$$\begin{array}{ccc}\hfill \delta {\dot{\mathit{q}}}_v& \approx & {\displaystyle \frac{1}{2}}\delta \mathit{\omega }\hfill \\ \hfill \delta {\dot{q}}_4& \approx & 0,\hfill \end{array}$$

(65)

where the first-order approximation of the attitude error matrix is given by the following:

$$\delta C\approx I_{3\times 3}-2[\delta {\mathit{q}}_v\times ].$$

(66)

Considering the same rate-integrating gyro sensor model defined in Equation (61) and the observed angular velocity definition in Equation (62), the angular velocity error is written as follows:

$$\delta \mathit{\omega }=(I_{3\times 3}-\delta C)\tilde{\mathit{\omega }}-(\delta \mathit{\beta }+{\mathit{\eta }}_v)=-2[(\widehat{\mathit{\omega }}+\widehat{\mathit{\beta }})\times ]\delta {\mathit{q}}_v-(\delta \mathit{\beta }+{\mathit{\eta }}_v),$$

(67)

where $\delta \mathit{\beta }=\mathit{\beta }-\delta C\widehat{\mathit{\beta }}$. The multiplicative extended Kalman filter error model is now given by the following (note: $\dot{\widehat{\mathit{\beta }}}=0$):

$$\begin{array}{ccc}\hfill \delta {\dot{\mathit{q}}}_v& \approx & -[(\widehat{\mathit{\omega }}+\widehat{\mathit{\beta }})\times ]\delta {\mathit{q}}_v-\frac{1}{2}(\delta \mathit{\beta }+{\mathit{\eta }}_v)\hfill \\ \hfill \delta \dot{\mathit{\beta }}& \approx & {\mathit{\eta }}_u+[\delta \mathit{\omega }\times ]\delta C\widehat{\mathit{\beta }}.\hfill \end{array}$$

(68)

3.3. Extended Kalman Filter Error Model

For a single sensor, we define the true and estimated body vectors as follows:

$$\begin{array}{ccc}\hfill \mathit{b}& =& C(\mathit{q})\mathit{r}\hfill \\ \hfill {\widehat{\mathit{b}}}^{-}& =& C({\widehat{\mathit{q}}}^{-})\mathit{r},\hfill \end{array}$$

(69)

where $\mathit{r}={\{x,y,z\}}^{\mathrm{T}}$ is the $3\times 1$ vector of the vehicle position in the earth-centered, earth-fixed (ECEF) coordinates, the true attitude matrix is $C(\mathit{q})=C(\delta \mathit{q})C({\widehat{\mathit{q}}}^{-})$, $C(\delta \mathit{q})$ is defined in Equation (37), and $C(\widehat{\mathit{q}})=\left[\begin{array}{c}({\widehat{q}}_4^2-{\widehat{\mathit{q}}}_v^{\mathrm{T}}{\widehat{\mathit{q}}}_v)I+2{\widehat{\mathit{q}}}_v\delta {\widehat{\mathit{q}}}_v^{\mathrm{T}}-2{\widehat{q}}_4[{\widehat{\mathit{q}}}_v\times ]\end{array}\right]$. Thus, the estimation error of the body vector is defined as follows:

$$\Delta \mathit{b}\equiv \mathit{b}-{\widehat{\mathit{b}}}^{-}=2[C({\widehat{\mathit{q}}}^{-})\mathit{r}\times ]\delta {\mathit{q}}_v.$$

(70)

The sensitivity matrix of n measurement sets is given by the following:

$$H=\left[\begin{array}{cc}2[C({\widehat{\mathit{q}}}^{-}){\mathit{r}}_1\times ]& 0_{3\times 3}\\ 2[C({\widehat{\mathit{q}}}^{-}){\mathit{r}}_2\times ]& 0_{3\times 3}\\ ⋮& ⋮\\ 2[C({\widehat{\mathit{q}}}^{-}){\mathit{r}}_n\times ]& 0_{3\times 3}\end{array}\right].$$

(71)

The EKF error model is given by the following:

$$\Delta \dot{\tilde{\mathit{x}}}=F(\widehat{\mathit{x}}(t),t)\Delta \tilde{\mathit{x}}(t)+G(t)\mathit{w}(t),$$

(72)

where $\Delta \tilde{\mathit{x}}(t)={\left[\delta {\mathit{q}}_v^{\mathrm{T}}\delta {\mathit{\beta }}^{\mathrm{T}}\right]}^{\mathrm{T}};\mathit{w}={\left[{\mathit{\eta }}_v^{\mathrm{T}}{\mathit{\eta }}_u^{\mathrm{T}}\right]}^{\mathrm{T}}$; and $F(\widehat{\mathit{x}}(t),t),G(t)$, and $Q(t)$ are given by

Table 2. The extended Kalman filter (EKF) model.

$\mathit{\delta }\mathit{\omega }=\mathit{\omega }-\widehat{\mathit{\omega }}$

$\mathit{\delta }\mathit{\omega }=\mathit{\omega }-\mathit{\delta }\mathit{C}\widehat{\mathit{\omega }}$

$\begin{array}{cc}F\hfill & =\left[\begin{array}{cc}-[\widehat{\mathit{\omega }}\times ]& -\frac{1}{2}{I}_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 3}\end{array}\right]\hfill \\ G\hfill & =\left[\begin{array}{cc}-\frac{1}{2}{I}_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& I_{3\times 3}\end{array}\right]\hfill \\ Q\hfill & =\left[\begin{array}{cc}{\sigma }_v^2{I}_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& {\sigma }_v^2{I}_{3\times 3}\end{array}\right]\hfill \end{array}$

$\begin{array}{cc}F\hfill & =\left[\begin{array}{cc}-[(\widehat{\mathit{\omega }}+\widehat{\mathit{\beta }})\times ]& -\frac{1}{2}{I}_{3\times 3}\\ 2[\widehat{\mathit{\beta }}\times ][(\widehat{\mathit{\omega }}+\widehat{\mathit{\beta }})\times ]& [\widehat{\mathit{\beta }}\times ]\end{array}\right]\hfill \\ G\hfill & =\left[\begin{array}{cc}-\frac{1}{2}{I}_{3\times 3}& 0_{3\times 3}\\ \widehat{\mathit{\beta }}\times ]& I_{3\times 3}\end{array}\right]\hfill \\ Q\hfill & =\left[\begin{array}{cc}{\sigma }_v^2{I}_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& {\sigma }_v^2{I}_{3\times 3}\end{array}\right]\hfill \end{array}$

:

Discrete-time attitude observation $n\times 1$ model at time $t_k$ is given by the following:

$${\tilde{\mathit{y}}}_k={\left\{\begin{array}{c}\left[C(\mathit{q}){\mathit{r}}_1\right]\\ \mathit{q}){\mathit{r}}_2]\\ ⋮\\ \mathit{q}){\mathit{r}}_n]\end{array}\right\}}_{t_k}+{\left\{\begin{array}{c}{\nu }_1\\ {\nu }_2\\ ⋮\\ {\nu }_n\end{array}\right\}}_{t_k}\equiv {\mathit{h}}_k({\widehat{\mathit{x}}}_k)+{\mathit{v}}_k,$$

(73)

where ${\mathit{v}}_k\sim \mathcal{N}(0,R_k)$ is zero-mean gaussian measurements noise with covariance error matrix $R_k$. Thus, the error state update is given by the following:

$$\Delta {\widehat{\tilde{x}}}_k^{+}=K_k[{\tilde{\mathit{y}}}_k-{\mathit{h}}_k({\widehat{\mathit{x}}}_k^{-})],$$

(74)

where $\Delta {\widehat{\tilde{x}}}_k^{+}={\left[\delta {\widehat{\tilde{q}}}_k^{+}\delta {\widehat{\tilde{\beta }}}_k^{+}\right]}^{\mathrm{T}}$, ${\tilde{\mathit{y}}}_k$ is the measurement output, ${\mathit{h}}_k({\widehat{\mathit{x}}}_k^{-})$ is the estimate output, and $K_k$ is Kalman gain, as given in Equation (75).

The EKF is implemented following the below sequential steps [30,31]:

• Initialization: at $t_0$ and for given initial states ${\mathit{x}}_0={[{\widehat{\mathit{q}}}_0,{\mathit{\beta }}_0]}^T$ and initial value of the covariance matrix $P_0$, the initial values are given by the following:

  $$\begin{array}{ccc}\hfill {\widehat{\mathit{x}}}^{-}(0)& =& E\left\{\mathit{x}(0)\right\}={\mathit{x}}_0\hfill \\ \hfill P^{-}(0)& =& E\left\{(\mathit{x}(0)-{\mathit{x}}_0){(\mathit{x}(0)-{\mathit{x}}_0)}^T\right\}=P_0,\hfill \end{array}$$

  (75)

  where the superscript (−) denotes priori values and $E\left\{\right\}$ is the expectation operator. Assume that $\mathit{x}(0)\sim \mathcal{N}(0,P(0))$.
• Gain: compute the Kalman gain matrix:

  $$K_k=P_k^{-}{H}_k^T({\widehat{\mathit{x}}}_k^{-}){\left[H_k({\widehat{\mathit{x}}}_k^{-})P_k^{-}{H}_k^T({\widehat{\mathit{x}}}_k^{-})+R_k\right]}^{-1},$$

  (76)

  where $H_k({\widehat{\mathit{x}}}_k^{-})\equiv \frac{\partial \mathit{h}}{\partial \mathit{x}}{|}_{{\widehat{\mathit{x}}}_k^{-}}$ is given in Equation (71).
• Update: update the state estimate ${\widehat{\mathit{x}}}_k^{+}$ and covariance $P_k^{+}$ at each measurement:

  $$\begin{array}{ccc}\hfill {\widehat{\mathit{x}}}_k^{+}& =& {\widehat{\mathit{x}}}_k^{-}+K_k[{\tilde{\mathit{y}}}_k-{\mathit{h}}_k({\widehat{\mathit{x}}}_k^{-})]\hfill \\ \hfill P_k^{+}& =& [I_{3\times 3}-K_k{H}_k({\widehat{\mathit{x}}}_k^{-})]P_k^{+},\hfill \end{array}$$

  (77)

  where the superscript (+) denotes posteriori values.
• Propagation: propagate both the state estimate ${\widehat{\mathit{x}}}_k$ and covariance $P_k$ using the posteriori estimate ${\widehat{\mathit{x}}}_k^{+}$ and posteriori covariance $P_k^{+}$. The estimated angular velocity, $\widehat{\mathit{\omega }}=\tilde{\mathit{\omega }}-\widehat{\mathit{\beta }}$, is used to propagate the quaternion kinematics:

  $$\begin{array}{ccc}\hfill \dot{\widehat{\mathit{q}}}& =& {\displaystyle \frac{1}{2}}\left[\begin{array}{cc}-[\mathit{\omega }\times ]& \widehat{\mathit{\omega }}\\ -{\widehat{\mathit{\omega }}}^{\mathrm{T}}& 0\end{array}\right]\widehat{\mathit{q}}\hfill \\ \hfill \dot{\widehat{\mathit{\beta }}}& =& 0\hfill \\ \hfill \dot{P}& =& FP+P{F}^T+GQ{G}^T,\hfill \end{array}$$

  (78)

  where the matrices F, G, and Q are given by Table 2.

3.4. Numerical Simulation

This section presents simulation results that utilize data provided by a star tracker to estimate the attitude of slow spinning spacecraft under the following scenario:

• Optical axis aligned with $\widehat{\mathit{z}}$ axis of the body reference frame.
• Sensor field of view: ${10}^{\circ }\times {12}^{\circ }$.
• Star catalog with magnitude threshold = 6.
• Observed stars affected by multiplicative Gaussian noise [32] due to centroid error, $3\sigma ={20}^{″}$.
• spacecraft is spinning about the $\widehat{\mathit{y}}$ axis with constant angular velocity 1.01 rad/s.
• Simulation time is 300 s with sampling frequency of 10 Hz.

The directions captured by the sensor field of view during the whole trajectory is shown in Figure 3. Figure 4 shows the attitude estimation error, which is defined as the principal angle between estimated and true DCM and is small and within the predicted covariance ($\pm 3\sigma$) error.

4. Optimal Tracking Control

A general scalar nonnegative attitude penalty function is utilized to formulate an optimal feedback control for the spacecraft tracking problem. This new variable yields identical performance index values, regardless of the attitude variables selected. The general final finite-time optimal control formulating is given by minimizing the following performance index [29]:

$$\Im ={\displaystyle \frac{1}{2}}\Phi (t_f,\delta \mathit{\omega }(t_f),\delta \mathit{\zeta }(t_f))+{\displaystyle \frac{1}{2}}{\int }_{t_0}^{t_f}L(\delta \mathit{\omega },\delta \mathit{\zeta },\mathit{u},t)dt,$$

(79)

which is subject to $\dot{\mathit{x}}={\left[\delta {\dot{\mathit{\omega }}}^{\mathrm{T}}\delta {\dot{\mathit{\zeta }}}^{\mathrm{T}}\right]}^{\mathrm{T}}=\mathit{f}(\delta \mathit{\omega },\delta \mathit{\zeta },\mathit{u})$, where $\delta \mathit{\omega }$ is the angular velocity error, $\delta \mathit{\zeta }$ is an arbitrary attitude representation of the attitude error, and the penalty functions are (using $\delta \mathit{\omega }(t_f)=\delta {\mathit{\omega }}_{t_f}$ and $\delta \mathit{\zeta }(t_f)=\delta {\mathit{\zeta }}_{t_f}$):

$$\begin{array}{ccc}\hfill \Phi (t_f,\delta \mathit{\omega }(t_f),\delta \mathit{\zeta }(t_f))& =& Q_{1}g(\delta {\mathit{\zeta }}_{t_f})+\delta {\mathit{\omega }}_{t_f}^{\mathrm{T}}{Q}_2\delta {\mathit{\omega }}_{t_f}\hfill \\ \hfill \mathrm{and}L(\delta \mathit{\omega },\delta \mathit{\zeta },\mathit{u},t)& =& Q_{3}g(\delta \mathit{\zeta })+\delta {\mathit{\omega }}^{\mathrm{T}}{Q}_4\delta \mathit{\omega }+{\mathit{u}}^{\mathrm{T}}R\mathit{u}.\hfill \end{array}$$

(80)

The weights $Q_1$ and $Q_3$ are scalars, and the weights $Q_2$, $Q_4$, and R are $3\times 3$ matrices. The $\mathit{f}(\delta \mathit{\omega },\delta \mathit{\zeta },\mathit{u})$ is the spacecraft error dynamics. The scalar function $g(\delta \mathit{\zeta })$ is a general nonnegative attitude penalty function. The function is chosen to produce the same cost for a given physical orientation [23,29]:

$$g(\left[\delta C(\delta \widehat{\mathit{e}},\delta \varphi )\right])={\displaystyle \frac{1}{4}}(3-\mathrm{tr}(\left[\delta C\right]))={sin}^2(\delta \varphi /2),$$

(81)

when using exact nonlinear attitude error kinematics, the orientation will work for large angles; $\delta \varphi =\pm {180}^{\circ }$. Therefore, the function is bounded $0\le g(\delta C)\le 1$ for all possible rotations. Thus, the attitude cost reaches its highest value at the maximum rotation angle. Defining the attitude cost function in terms of the DCM makes it universally valid for arbitrary choice of attitude coordinates. It can be simply parameterized by any other attitude coordinate. The universal quadratic penalty function for arbitrary attitude error representations is given in

Table 3. Universal penalty function for attitude error.

Attitude Parameter	Penalty Function
DCM	$g(\delta C)={\displaystyle \frac{1}{4}}(3-\mathrm{tr}(\delta C))$
Quaternion	$g(\delta C(\delta \mathit{q}))=\delta q_1^2+\delta q_2^2+\delta q_3^2$
CRPs	$g(\delta C(\delta \mathit{\rho }))={\displaystyle \frac{\delta {\mathit{\rho }}^{\mathrm{T}}\delta \mathit{\rho }}{1+\delta {\mathit{\rho }}^{\mathrm{T}}\delta \mathit{\rho }}}$
MRPs	$g(\delta C(\delta \mathit{\sigma }))=4{\displaystyle \frac{\delta {\mathit{\sigma }}^{\mathrm{T}}\delta \mathit{\sigma }}{{(1+\delta {\mathit{\sigma }}^{\mathrm{T}}\delta \mathit{\sigma })}^2}}$
Euler Angles${}^{*}$	$g(\delta C(\delta \mathit{\theta }))={\displaystyle \frac{1}{4}}[3-(1+cos\delta {\theta }_2)cos(\delta {\theta }_1+\delta {\theta }_3)-cos\delta {\theta }_2]$
Principal angle/axis	$g(\delta C(\delta \widehat{\mathit{e}},\delta \varphi ))={sin}^2(\delta \varphi /2)$
Cayley-Klein	$g(\delta C(\delta K))=1-\frac{1}{4}\mathrm{tr}{(\delta K)}^2$

${}^{*}$ For the 3-1-3 rotation sequence.

. This penalty function returns the same cost for a given physical attitude, thereby eliminating the dependency of the optimal control solution on the choice of the attitude coordinate.

4.1. Reference Angular Velocity Defined in the Body Attitude Frame

The reference ${\mathit{\omega }}_r$ and current $\mathit{\omega }$ angular velocities are expressed in the same coordinate frames; i.e., $\delta \mathit{\omega }=\mathit{\omega }-{\mathit{\omega }}_r$. It can be shown that the general expression of the attitude error kinematics of this set can be given as follows:

$$\delta \dot{\mathit{\zeta }}=\left[{\mathit{f}}_{\delta \mathit{\zeta }}(\delta \mathit{\zeta })\right]\delta \mathit{\omega }-[{\mathit{\omega }}_r\times ]\delta \mathit{\zeta },$$

(82)

with initial state $\delta \mathit{\zeta }(t_0)=\delta {\mathit{\zeta }}_0$. The Hamiltonian H for this system of equations is defined, for the given optimal control problem in Equations (79) and (80), as follows:

$$\begin{array}{cc}H=\hfill & {\displaystyle \frac{1}{2}}(Q_{3}g(\delta \mathit{\zeta })+\delta {\mathit{\omega }}^{\mathrm{T}}{Q}_4\delta \mathit{\omega }+{\mathit{u}}^{\mathrm{T}}R\mathit{u})-\hfill \\ \hfill & {\mathit{\lambda }}_{\delta \omega }^{\mathrm{T}}{I}^{-1}([{\mathit{\omega }}_r\times ]I-[(I{\mathit{\omega }}_r)\times ])\delta \mathit{\omega }+[\delta \mathit{\omega }\times ]I\delta \mathit{\omega }-\mathit{u}+I\dot{{\mathit{\omega }}_r}+[{\mathit{\omega }}_r\times ]I{\mathit{\omega }}_r)+\hfill \\ \hfill & {\mathit{\lambda }}_{\delta \zeta }^{\mathrm{T}}(\left[{\mathit{f}}_{\delta \mathit{\zeta }}(\delta \mathit{\zeta })\right]\delta \mathit{\omega }-[{\mathit{\omega }}_r\times ]\delta \mathit{\zeta }),\hfill \end{array}$$

(83)

where ${\mathit{\lambda }}_{\delta \omega }$ and ${\mathit{\lambda }}_{\delta \zeta }$ are the co-state variables for the angular velocity error and the attitude error, respectively. Invoking the standard necessary condition for optimality, the co-state differential equations are given by the following:

$$\begin{array}{c}{\dot{\mathit{\lambda }}}_{\delta \omega }=-Q_4\mathit{\delta }\omega -{\left[{\mathit{f}}_{\delta \mathit{\zeta }}(\delta \mathit{\zeta })\right]}^{\mathrm{T}}{\mathit{\lambda }}_{\delta \zeta }-(I[\mathit{\delta }\omega \times ]-[(I\delta \mathit{\omega })\times ]-{([{\mathit{\omega }}_r\times ]I-[(I{\mathit{\omega }}_r)\times ])}^{\mathrm{T}})I^{-1}{\mathit{\lambda }}_{\delta \omega }\hfill \\ {\dot{\mathit{\lambda }}}_{\delta \zeta }=-{\displaystyle \frac{1}{2}}{Q}_3{\displaystyle \frac{\partial g}{\partial (\delta \mathit{\zeta })}}-{\displaystyle \frac{\partial }{\partial (\delta \mathit{\zeta })}}{(\left[{\mathit{f}}_{\delta \mathit{\zeta }}(\delta \mathit{\zeta })\right]\delta \mathit{\omega })}^{\mathrm{T}}{\mathit{\lambda }}_{\delta \zeta }-[{\mathit{\omega }}_r\times ]{\mathit{\lambda }}_{\delta \zeta },\hfill \end{array}$$

(84)

The two co-state differential equations are integrated backward in time with given terminal values ${\mathit{\lambda }}_{\delta \omega }(t_f)=\frac{\partial \Phi }{\partial (\delta \mathit{\omega })}{|}_{t_f}$ and ${\mathit{\lambda }}_{\delta \zeta }=\frac{\partial \Phi }{\partial (\delta \mathit{\zeta })}{|}_{t_f}$. The optimal control is given by the first-order necessary conditions for an extremum, $H_u=0$, leading to $\mathit{u}=-{(IR)}^{-1}{\mathit{\lambda }}_{\delta \omega }$.

4.2. Reference Angular Velocity Defined in the Reference Attitude Frame

The reference and current angular velocities are expressed in different coordinate frames, i.e., $\delta \mathit{\omega }=\mathit{\omega }-\delta C{\mathit{\omega }}_r$. Therefore, this definition explicitly computes the angular velocity error in the current body frame. As an important result of this set, the expressions of the attitude error kinematics follow the attitude kinematics equation for any given attitude representation choice. Therefore, the general expression of the attitude error kinematics of this set can be given as follows:

$$\delta \dot{\mathit{\zeta }}=\left[{\mathit{f}}_{\delta \mathit{\zeta }}(\delta \mathit{\zeta })\right]\delta \mathit{\omega },$$

(85)

with initial state $\delta \mathit{\zeta }(t_0)=\delta {\mathit{\zeta }}_0$. The Hamiltonian H for this system of equations is as follows:

$$\begin{array}{cc}H=\hfill & {\displaystyle \frac{1}{2}}(Q_{3}g(\delta \mathit{\zeta })+\delta {\mathit{\omega }}^{\mathrm{T}}{Q}_4\delta \mathit{\omega }+{\mathit{u}}^{\mathrm{T}}R\mathit{u})-\hfill \\ \hfill & {\mathit{\lambda }}_{\delta \omega }^{\mathrm{T}}{I}^{-1}(\left([\mathit{\delta }C{\omega }_r\times ]I-[\mathit{I}\delta C{\omega }_r\times ]\right)\delta \mathit{\omega }+[\mathit{\delta }\omega \times ]I\delta \mathit{\omega }-\mathit{u}-\hfill \\ \hfill & I[\mathit{\delta }\omega \times ]\delta C{\mathit{\omega }}_r+I{\dot{\mathit{\omega }}}_r+[\mathit{\delta }C{\mathit{\omega }}_r\times ]J\delta C{\mathit{\omega }}_r)+{\mathit{\lambda }}_{\delta \zeta }^{\mathrm{T}}\left[{\mathit{f}}_{\delta \mathit{\zeta }}(\delta \mathit{\zeta })\right]\delta \mathit{\omega }.\hfill \end{array}$$

(86)

Invoking the standard necessary condition for optimality, the co-state differential equations are given by the following:

$$\begin{array}{c}{\dot{\mathit{\lambda }}}_{\delta \omega }=-Q_4\mathit{\delta }\omega -{\left[{\mathit{f}}_{\delta \mathit{\zeta }}(\delta \mathit{\zeta })\right]}^{\mathrm{T}}{\mathit{\lambda }}_{\delta \zeta }-(I[\mathit{\delta }\omega \times ]-[(I\delta \mathit{\omega })\times ]-{([\delta C{\mathit{\omega }}_r\times ]I-[(I\delta C{\mathit{\omega }}_r)\times ])}^{\mathrm{T}}+[\mathit{\delta }C{\omega }_r\times ]I)I^{-1}{\mathit{\lambda }}_{\delta \omega }\hfill \\ {\dot{\mathit{\lambda }}}_{\delta \zeta }=-{\displaystyle \frac{1}{2}}{Q}_3{\displaystyle \frac{\partial g}{\partial (\delta \mathit{\zeta })}}-{\displaystyle \frac{\partial }{\partial (\delta \mathit{\zeta })}}{(\left[{\mathit{f}}_{\delta \mathit{\zeta }}(\delta \mathit{\zeta })\right]\delta \mathit{\omega })}^{\mathrm{T}}{\mathit{\lambda }}_{\delta \zeta }-[{\mathit{\omega }}_r\times ]{\mathit{\lambda }}_{\delta \zeta }-{\displaystyle \frac{\partial }{\partial (\delta \mathit{\zeta })}}{\left[\delta \dot{}\mathit{\omega }\right]}^{\mathrm{T}}{\mathit{\lambda }}_{\delta \omega },\hfill \end{array}$$

(87)

The optimal control is given by the first-order necessary conditions for an extremum, $H_u=0$, leading to $\mathit{u}=-{(IR)}^{-1}{\mathit{\lambda }}_{\delta \omega }$. Note the last term in the (${\dot{\mathit{\lambda }}}_{\delta \zeta }$) expression involves calculating the partial derivative of the angular velocity error kinematic, ${\displaystyle \frac{\partial }{\partial (\delta \mathit{\zeta })}}\left[\delta \dot{}\mathit{\omega }\right]$, which obviously leads to difficult math. This step is also required when performing coordinate mapping between the DCM into other attitude parameters.

4.3. Numerical Simulation

This section presents simulation results of a fixed final-time and final-state open-loop optimal control solution for the spacecraft tracking problem. Modified Rodrigues parameters are used for the attitude motion. The initial and final state variable conditions for this example are given in

Table 4. Initial/boundary conditions.

Time (s)	Attitude Error (MRPs) $\mathit{\delta }\mathit{\zeta }$	Angular Velocity Error $\mathit{\delta }\mathit{\omega }$ (rad/s)
0	$\frac{1}{\sqrt{3}}{[1,1,1]}^{\mathrm{T}}$	${[-0.1,-0.2,-0.3]}^{\mathrm{T}}$
25	${[0,0,0]}^{\mathrm{T}}$	${[0,0,0]}^{\mathrm{T}}$

. The spacecraft moment of inertia tensor is given in

Table 5. The spacecraft principal moment of inertia components (kg·m${}^2$).

$I_1$	86.215
$I_2$	85.070
$I_3$	113.565

. For simplicity, the weighting matrices are $Q_1=0$, $Q_3=1$, $Q_2=0_{3\times 3}$, and $Q_4=R=I_{3\times 3}$. However, one can sweep those penalties to obtain different solutions sets. This example stands to solve the open-loop optimal control problem for the spacecraft tracking problem using arbitrary attitude representation. We consider the universal performance index given in Table 3. The state and co-state differential equations are solved in a Boundary Value Problem (BVP) framework using a shooting method (MATLAB fsolve) [29,33]. The optimal open-loop solution is shown in Figure 5. The time history of the optimal trajectory and control is shown in Figure 5a for MRPs representation. The trajectory is controlled to drive the spacecraft for a given initial state error, $\delta \mathit{\omega }(0)$ and $\delta \mathit{\zeta }(0)$, to rest at zero attitude error after 25 seconds. It is noted that the optimal open-loop solution obtained for various attitude representation produces the same angular displacement $\delta \varphi$ and total cost, as given in Figure 5b.

5. Conclusions

Full analytical derivations of attitude error kinematics equations have been presented. Compact forms of attitude error kinematics equations are derived for various attitude parameterizations. The attitude error is defined as the rotation error between the true and estimated orientations. Several attitude error representations are developed for describing the orientation error kinematics. Two approaches to attitude error kinematics are introduced. The first one considers the estimated angular velocity defined in the true body axes, while in the second one, the estimated angular velocity is defined in the estimated body axes. These two angular velocity definitions are usually adopted in simulations and in real estimation/control applications, respectively. These two nonlinear kinematic models are valid for arbitrarily large relative rotations and rotation rates. These results are expected to be broadly useful for generalizing extended Kalman filtering formulations and optimal control tracking problems.