Unscented Kalman filter for spacecraft attitude estimation using modified Rodrigues parameters and real data

Abstract

In this paper, a sigma-point Kalman filter formulation for attitude estimation is derived using the modified Rodrigues parameters and real data of attitude sensors. The unscented Kalman filter algorithm is used for attitude estimation and the gyro-based model is considered for attitude propagation. In this study, the attitude of satellite is estimated using real data supplied by gyros, Earth sensors and Sun sensors that are on board of the CBERS-2 (China Brazil Earth Resources Satellite). The results show that even with a sparse set of measures, the UKF with MRP shows results similar to those obtained when directly uses the Euler angles.

1 Introduction

In problems that involve spacecraft attitude estimation, the quaternion has shown wide applications because it is free from singularity with a bilinear kinematic equation. However, the redundant property of the quaternion renders the singularity of the covariance matrix and requires to normalize the estimated quaternion. The usual approach to satisfying the constraint is to estimate an error quaternion at each measurement update and then form the true quaternion estimate from the composition of the estimated error quaternion with the predicted quaternion based on the state transition matrix. We assume that small errors allow for the first three components of the quaternion to be estimated independently of the fourth component, which essentially amounts to a linearization using small angle assumptions. This technique is characterized by way of three-dimensional parameterization. Concerning these parameterizations, the modified Rodrigues parameters (MRP) have drawn ever-increasing attention because of their simplicity and high efficiency. The UKF is a more robust estimation method and in several studies proved to be more efficient when compared with the extended Kalman filter. In this paper, we analyze the UKF behavior when MRP are used to parameterize the attitude. In addition, the results will be evaluated when the estimated data are subjected to real data which are not aware of the nature of the errors that are present in the measurements. The attitude estimated by the UKF with MRP will be compared with results obtained directly by Euler angles to highlight the main advantages of using such an approach.

2 Attitude representation based on the modified Rodrigues parameters

The MRPs are defined in terms of the quaternions $\mathbf {q}=[\varrho \, q_{4}]^{T}$ as Crassidis and Markley (2003)

$$\begin{aligned} \mathbf {p} = \frac{\varrho }{1+q_{4}} = \mathbf {e} \tan \left( \frac{\varphi }{4}\right) , \end{aligned}$$

(1)

where $\mathbf {e}$ is the principal rotation axis, $\varphi$ the principal rotation angle, $\varrho$ the vector part of the quaternion and $q_{4}$ the scalar part of the quaternion, defined by

$$\begin{aligned} \varrho = \left[ q_{1} \, q_{2} \, q_{3}\right] ={\mathbf {e}} \sin \left( \varphi /2\right) . \end{aligned}$$

(2)

$$\begin{aligned} q_{4} = \cos \left( \varphi /2\right) . \end{aligned}$$

The magnitude of $\mathbf {p}$ is given by $|\mathbf {p}|$. Then, $|\mathbf {p}|\rightarrow \infty$ when $\varphi \rightarrow 2\pi$, meaning that MRP are singular at $2\pi$. Since we only use a three-component representation for the attitude errors, the singularity is never encountered in practice. The updates are performed using quaternion multiplication, leading to a natural way of maintaining the normalization constraint.

In this work, an error quaternion defined by MRP will be considered and is given by Crassidis et al. (2007)

$$\begin{aligned} \mathbf {\delta p} = f \frac{\delta \varrho }{a+\delta q_{4}} \end{aligned}$$

(3)

with $\mathbf {\delta q} \equiv [\delta \varrho ^{T} \, \delta q_{4}]^{T}$, a is a parameter from 0 to 1, and $f=2(a+1)$ is a scalar factor.

The inverse conversion of $\mathbf {\delta p}$ to $\mathbf {\delta q}$ is given by

$$\begin{aligned} \begin{aligned} \delta q_{4}&= \frac{-a \Vert \delta \mathbf {p} \Vert ^{2} + f \sqrt{f^{2}+(1-a^{2})\Vert \delta \mathbf {p} \Vert ^{2}}}{f^{2}+\Vert \delta \mathbf {p} \Vert ^{2}} \\ \delta \mathbf {\varrho }&= f^{-1} (a+\delta q_{4})\delta \mathbf {p}. \end{aligned} \end{aligned}$$

(4)

3 The measurement system of the satellite

In problems of attitude, estimation is required to get results with high precision. In these cases, data fusion is required to process the information collected from different sensors. In this work measurements supplied by gyroscopes, Earth sensors and solar sensors will be considered. These sensors are on board CBERS-2 satellite.

3.1 The measurement equation for gyros

The rate-integration gyros (RIGs) are used to measure the angular velocities of the roll, pitch and yaw axes of the satellite. The mathematical model of the measured components of the angular velocity of the satellite are given by Silva et al. (2015):

$$\begin{aligned} \mathbf {\omega }_{i} = \mathbf {g}-\mathbf {\varepsilon }_{i}-\mathbf {\eta }, \end{aligned}$$

(5)

where $\mathbf {g}(t)$ is the output vector of the gyroscope, $\mathbf {\varepsilon }_{i}$ are components of bias of the gyroscope in axes x, y, z, and $\mathbf {\eta }$ represents a Gaussian white noise process covering all the remaining unmodeled effects.

3.2 The measurement model for infrared Earth sensors (IRES)

In this work, only two Earth sensors are used, with one measuring the roll angle and the other measuring the pitch angle. The measurement equations for the Earth sensors are given as Silva et al. (2015)

$$\begin{aligned} \begin{array}{l} \phi _H \,=\,\phi \,\,+\,\,\nu _\phi \\ \theta _H \,=\,\theta \,\,+\,\,\nu _\theta \\ \end{array}, \end{aligned}$$

(6)

where $\nu _\phi$ and $\nu _\theta$ represent the Gaussian white noise related to small remaining effects of misalignment during installation and/or assembly of sensors.

3.3 The measurement model for digital Sun sensor (DSS)

The digital Sun sensors do not provide direct measurements, but coupled angle of pitch ($\alpha _\theta )$ and yaw ($\alpha _\psi )$. The measurement equations for the sun sensor are established as follows (Silva et al. 2015):

$$\begin{aligned} \alpha _\psi \,=\,tg^{-1}\,\,\left( {\frac{-\,S_y }{S_x \,\cos \,60^{\circ }\,\,+\,\,S_z \,\cos \,150^{\circ }}} \right) + \nu _{\alpha _{\psi }}, \end{aligned}$$

(7)

when $\left| {\,S_x \,\cos \,60^{\circ }\,\,+\,\,S_z \,\cos \,150^{\circ }\,} \right| \,\,\ge \,\,\cos \,60^{\circ }$.

$$\begin{aligned} \alpha _\theta \,\,=\,\,24^{\circ }\,\,-\,\,tg^{-1}\,\left( {\frac{S_x }{S_z }} \right) + \nu _{\alpha _{\theta }}, \end{aligned}$$

(8)

when $\left| {\,24^{\circ }\,\,-\,\,tg^{-1}\,\left( {\frac{S_x }{S_z }}\right) } \right| \,\,<\,\,60^{\circ }$. The $S_{x}$, $S_{y}$ and $S_{z}$ are the components of the unit vector associated with the sun vector in the satellite system. The Gaussian white noise is represented by $\nu _{\alpha _{\psi }}$ and $\nu _{\alpha _{\theta }}$ and represent small effect remnants of misalignment during installation and/or by sensor assembly.

4 Unscented Kalman filter formulation

In this section, the necessary steps to the formulation UKF will be presented when the MRP are considered to parameterize the attitude during the estimation process.

The first step:

The filter is initialized with the state vector information $\hat{\mathbf {x}}^{+}_{k}=\left[ \mathbf {q}^{+}_{k} \, {\beta }^{+}_{k}\right] ^{T}$ and the covariance matrices ${\hat{\mathbf {P}}}$, ${\hat{\mathbf {R}}}$ and ${\hat{\mathbf {Q}}}$ at the initial time $k=0$.

The second step:

Before propagating from time step k to $k+1$, it is necessary to compute the following set of sigma points (Julier and Uhlmann 2004):

$$\begin{aligned} \begin{aligned} \mathbf {\chi }_{k}(0)&=\hat{\mathbf {x}}^{+}_{k} \\ \mathbf {\chi }_{k}(i)&=\hat{\mathbf {x}}^{+}_{k} \pm \sqrt{(n+\lambda )[P^{+}_{k}+Q^{+}_{k}]}_{i}, \end{aligned} \end{aligned}$$

(9)

with $\sqrt{(n+\lambda )[P^{+}_{k}+Q^{+}_{k}]}_{i}$ the ith column of the matrix square root of $(n+\lambda )[P^{+}_{k}+Q^{+}_{k}]$.

From Eq. (9), a new state vector composed of $\delta {\hat{\mathbf {p}}}$ is defined which represents the attitude error quaternion, and $\hat{\mathbf {\beta }}$ representing the gyro bias. The state vector will be defined by Crassidis and Markley (2003)

$$\begin{aligned} \begin{aligned} \mathbf {\chi }_{k}(0)&= \hat{\mathbf {x}}^{+}_{k} \equiv [\delta \hat{\mathbf {p}}^{+}_{k} \, \hat{\mathbf {\beta }}^{+}_{k}]^{T} \\ \mathbf {\chi }_{k}(i)&\equiv \left[ \mathbf {\chi }^{\delta p}_{k}(i) \, \mathbf {\chi }^{\beta }_{k}(i)\right] ^{T} \quad \, i = 0,1,\ldots ,12. \end{aligned} \end{aligned}$$

(10)

From Eq. (4) and from the new state vector, given by Eq. (10), the sigma point matrix of error quaternion associated with MRP is obtained for $i=1, 2,\ldots , 12$, i.e., $\delta \mathbf {q}^{+}_{k}(i)\equiv [\delta \mathbf {\varrho }^{+T}_{k}(i) \, \delta \mathbf {q}^{+}_{4k}(i)]^{T}$. In this way, we can define the quaternion sigma point from

$$\begin{aligned} \begin{aligned} {\hat{\mathbf {q}}}^{+}_{k}(0)&={\hat{\mathbf {q}}}^{+}_{k}\\ {\hat{\mathbf {q}}}^{+}_{k}(i)&=\delta \mathbf {q}^{+}_{k}(i) \otimes {\hat{\mathbf {q}}}^{+}_{k} \quad \, i = 0,1,\ldots ,12. \end{aligned} \end{aligned}$$

(11)

The third step:

To propagate from time step k to $k+1,$ the state vector composed of attitude described by MPR, it is necessary to propagate the ${\hat{\mathbf {q}}}^{+}_{k}(i)$, $\delta \mathbf {q}^{+}_{k}(i)$ and finally the $\mathbf {\chi }^{\delta p}_{k}(i)$. The ${\hat{\mathbf {q}}}^{-}_{k}(i)$ propagation is given by

$$\begin{aligned} {\hat{\mathbf {q}}}^{-}_{k+1}(i) = \Phi ({\hat{\mathbf {\omega }}}^{+}_{k}) {\hat{\mathbf {q}}}^{+}_{k} \,\quad i = 0,1,\ldots ,12, \end{aligned}$$

(12)

with the transition matrix $\Phi ({\hat{\mathbf {\omega }}}^{+}_{k}) = \cos \left( 0.5 \Vert {\hat{\mathbf {\omega }}}^{+}_{k}\Vert \Delta t \right) \mathbb {I}_{4} + \sin \left( 0.5 \Vert {\hat{\mathbf {\omega }}}^{+}_{k}\Vert \Delta t\right) \Omega ({\hat{\mathbf {\omega }}}^{+}_{k})/\Vert {\hat{\mathbf {\omega }}}^{+}_{k}\Vert ,$ where $\Omega$ is the $4\times 4$ antisymmetric matrix (Lefferts et al. 1982) and ${\hat{\mathbf {\omega }}}^{+}_{k}(i)=\tilde{\mathbf {\omega }}_{k}-\mathbf {\chi }^{\beta }_{k}(i)$.

The $\delta \mathbf {q}^{-}_{k}(i)$ propagation is obtained by

$$\begin{aligned} \delta \mathbf {q}^{-}_{k+1}(i)={\hat{\mathbf {q}}}^{-}_{k+1}(i) \otimes [{\hat{\mathbf {q}}}^{-}_{k+1}(0)]^{-1} \quad \, i = 0,1,\ldots ,12. \end{aligned}$$

(13)

The last step is to propagate the sigma points related to attitude, $\mathbf {\chi }^{\delta p}_{k}$, and gyro bias $\mathbf {\chi }^{\beta }_{k+1}$, by

$$\begin{aligned} \begin{aligned} \mathbf {\chi }^{\delta p}_{k+1}(0)&=0 \\ \mathbf {\chi }^{\delta p}_{k+1}(i)&= f \left[ \delta \mathbf {\varrho }^{-}_{k+1}(i) /(a + \delta \mathbf {q}^{-}_{4k+1}(i))\right] \quad \, i = 1, 2,\ldots ,12\\ \mathbf {\chi }^{\beta }_{k+1}(j)&=\mathbf {\chi }^{\beta }_{k}(j)\quad \, j = 0,1,\ldots ,12. \end{aligned} \end{aligned}$$

(14)

The predicted mean of the state vector and the predicted covariance are given by

$$\begin{aligned} \begin{aligned} \hat{\mathbf {x}}^{-}_{k+1}&= \frac{1}{n+\lambda }\left\{ \lambda \chi _{k+1}(0) + \frac{1}{2}\sum _{i=1}^{2n} \chi _{k+1}(i)\right\} ,\\ P_{k+1}^{-}&=\frac{1}{n+\lambda }\left\{ \lambda \left[ \chi _{k+1}(0)-\hat{\mathbf {x}}^{-}_{k+1}\right] \left[ \chi _{k+1}(0)-\hat{\mathbf {x}}^{-}_{k+1}\right] ^{T} \right. \\&\left. \quad \,\, + \frac{1}{2} \sum _{i=1}^{2n}\left[ {\chi }_{k+1}(i)-\hat{\mathbf {x}}^{-}_{k+1}\right] \left[ \chi _{k+1}(i)-\hat{\mathbf {x}}^{-}_{k+1}\right] ^{T}\right\} +\mathbf {Q}_{k}. \end{aligned} \end{aligned}$$

(15)

The fourth step:

From the propagated ${\hat{\mathbf {q}}}^{-}_{k+1}(i),$ it is possible to calculate the mean observation and covariance related to observations, respectively, by Crassidis and Markley (2003)

$$\begin{aligned} \begin{aligned} \hat{\mathbf {y}}_{k+1}^{-}&= \frac{1}{n+\lambda } \left\{ \lambda \mathbf {Y}_{k+1}(0)+\frac{1}{2} \sum _{i=1}^{2n} \mathbf {Y}_{k+1i}\right\} \\ P^{yy}_{k+1}&= \frac{1}{n+\lambda } \left\{ \lambda \left[ \mathbf {Y}_{k+1}(0) - \hat{\mathbf {y}}_{k+1}^{-}\right] \left[ \mathbf {Y}_{k+1}(0) - \hat{\mathbf {y}}_{k+1}^{-}\right] ^{T} \right. \\&\left. \quad \,\, + \frac{1}{2} \sum _{i=1}^{2n} \left[ \mathbf {Y}_{k+1}(i) - \hat{\mathbf {y}}_{k+1}^{-}\right] \left[ \mathbf {Y}_{k+1}(i) - \hat{\mathbf {y}}_{k+1}^{-}\right] ^{T}\right\} + \mathbf {R}_{k+1}, \end{aligned} \end{aligned}$$

(16)

with $\mathbf {Y}_{k+1}(i)=h[{\hat{\mathbf {q}}}^{-}_{k+1}(i),k]$, $\mathbf {h}$ is the function that represents the sensors model and $R_{k}$ represents the measurement error covariance matrix.

Finally, the update of the state vector and the covariance is given, respectively, by the following formulation (Garcia et al. 2012):

$$\begin{aligned} \begin{aligned} \hat{\mathbf {x}}_{k+1}^{+}&=\hat{\mathbf {x}}_{k+1}^{-}+K_{k+1}[\mathbf {y}_{k+1}-\hat{\mathbf {y}}_{k+1}^{-}], \\ \hat{P}_{k+1}^{+}&= \hat{P}_{k+1}^{-} - K_{k+1} P^{yy}_{k+1} K_{k+1}^{T}, \end{aligned} \end{aligned}$$

(17)

where the Kalman gain, $K_{k+1}$, is given by

$$\begin{aligned} \begin{aligned} K_{k+1}&= P^{xy}_{k+1} (P^{yy}_{k+1})^{-1} \\ P^{xy}_{k+1}&= \frac{1}{n+\lambda } \left\{ \lambda \left[ {\chi }_{k+1}(0) - \hat{\mathbf {x}}_{k+1}^{-}\right] \left[ \mathbf {Y}_{k+1}(0) - \hat{\mathbf {y}}_{k+1}^{-}\right] ^{T} \right. \\&\left. \quad \,\, + \frac{1}{2} \sum _{i=1}^{2n} \left[ {\chi }_{k+1}(i) - \hat{\mathbf {x}}_{k+1}^{-}\right] \left[ \mathbf {Y}_{k+1}(i) - \hat{\mathbf {y}}_{k+1}^{-}\right] ^{T}\right\} . \end{aligned} \end{aligned}$$

(18)

Note that $\hat{\mathbf {x}}_{k+1}^{+} \equiv [\delta \hat{\mathbf {p}}_{k+1}^{+T} \, {\hat{\mathbf {\beta }}}_{k+1}^{+T}]^{T}$. Before the next propagation, it is necessary to update $\delta \mathbf {q}^{+}_{k+1}$ and $\mathbf {q}^{+}_{k+1}$ from Eq. (4) and (11), respectively. Lastly, $\delta \hat{\mathbf {p}}^{+}_{k+1}(0)$ is taken as zero and starts the next iteration.

5 Results

The aim of this work is to evaluate the behavior of the UKF algorithm adapted to use the MRP when real data of attitude sensors feed the estimator. The data sets are related to the sensors that are on board the CBERS-2 satellite. They are: infrared Earth sensor, digital Sun sensor and gyroscope. For analysis, a set of 54 measurements from 13 h 46 min 25 s until 13 h 55 min 27 s was used, with an interval of measures of 10 s on average. The measurements of the sensors DSS, IRES and gyroscope used in this paper are shown in Figs. 1, 2, 3, 4 and 5.

Fig. 1

Representation of real measurements supplied by digital Sun sensors from CBERS-2

Fig. 2

Representation of real measurements supplied by infrared Earth sensors from CBERS-2

Fig. 3

Representation of real measurements supplied by gyroscope from CBERS-2

Fig. 4

Representation of real measurements supplied by the gyroscope from CBERS-2

Fig. 5

Representation of real measurements supplied by the gyroscope from CBERS-2

It is known that the use of real data imposes certain difficulties, such as not being able to compare the result with the expected value (real value). To validate the results of this study, the results obtained in Garcia et al. (2012) will be used as reference. Table 1, 2, 3 and 4 shows the input data sets used in the estimator.

Table 1 Components of the state vector $\hat{\mathbf {x}}^{+}_{0}$

Table 2 Diagonal components of the covariance matrix $\hat{\mathbf {P}}^{+}_{0}$

Table 3 Diagonal components of the covariance matrix $\hat{\mathbf {Q}}^{+}_{0}$

Table 4 Diagonal components of the covariance matrix $\hat{\mathbf {R}}^{+}_{0}$

It is observed in Figs. 6 and 7 that during the period considered, attitude behavior estimated by the UKF with MRP is in agreement with the reference (Euler angles). The average of the estimated values for roll and pitch in both parameterizations were around $-0.47^{\circ }$ $-0.45^{\circ }$. For the yaw angle, Fig. 8, the estimate suffers a nonrandom variation, and the behavior is noticed in both parameterizations [behavior expected in other cases, Garcia et al. (2011)]. The average estimated yaw with MRP and Euler was $-1.49^{\circ }$ and $-1.51^{\circ }$, respectively.

Fig. 6

Roll estimated by representations of attitude MRP and Euler angles

Fig. 7

Pitch estimated by representations of attitude MRP and Euler angles

Fig. 8

Yaw estimated by representations of attitude MRP and Euler angles

Figures 9, 10 and 11 indicate that the data set considered is not enough to observe convergence in bias. Yet, the results of bias components on the axis x, y and z are similar to those obtained when compared with the reference (Euler angles). The average of the estimate of each component of the bias in x, y and z considering MRP was $5.57^{\circ }$, $4.9^{\circ }$ and $2.32^{\circ }$, respectively. In Figs. 12, 13 and 14, it is observed that, despite that the bias did not reach the state of convergence, the error in estimating the bias decreases in this period.

Fig. 9

Bias of gyro estimated in the x axis by representations of the attitude MRP and Euler angles

Fig. 10

Bias of gyro estimated in the y axis by representations of the attitude MRP and Euler angles

Fig. 11

Bias of gyro estimated in the z axis by representations of the attitude MRP and Euler angles

The Table 5 shows the CPU time spent for processing the measurements of the sensors by UKF in the different approaches used to estimate the attitude. 100 iterations for each approach was performed for a more reliable result. It may be noted that, although the processing time consumed by MRP exceeded the Euler, this increase is not directly proportional to the number of operations that are performed in MRP. The spent CPU via MRP is not 2 times the corresponding in Euler angles also well suited for real time processing while preserving the advantage of being a free attitude representation singularity and less prone to divergence due to non-linearities.

Fig. 12

Covariance estimated for bias in the x axis by representations of attitude MRP and Euler angles

Fig. 13

Covariance estimated for bias in the y axis by representations of attitude MRP and Euler angles

Fig. 14

Covariance estimated for bias in the z axis by representations of attitude MRP and Euler angles

6 Final comments

Real measurements generally bring unforeseen problems in simulations, requiring robust implementations of the estimation algorithms. This work was faced with low sampling and other difficulties of diverse and unknown sources, such as misalignments, drifts, systematic errors and unforeseen noise. Taking into account these difficulties, the purpose of this study was to analyze the UKF behavior when the MRP was used to parameterize the satellite attitude when real data are considered in the estimation process. The results showed that although the processing time MRP might be higher than that obtained when considering directly the Euler angles in the estimation process, the results with MRP are close to those obtained by reference. In problems involving estimation in real time, the filter processing time and parameterization of attitude chosen are of great importance. In this respect, the biggest advantage of choice for MRP is that their equations avoid singularities present in the direct use of Euler angles.

Table 5 Estimated time of processing of measures of attitude sensors by MRP and Euler angles