An Improved Invariant Kalman Filter for Lie Groups Attitude Dynamics with Heavy-Tailed Process Noise

Abstract

Attitude estimation is a basic task for most spacecraft missions in aerospace engineering and many Kalman type attitude estimators have been applied to the guidance and navigation of spacecraft systems. By building the attitude dynamics on matrix Lie groups, the invariant Kalman filter (IKF) was developed according to the invariance properties of symmetry groups. However, the Gaussian noise assumption of Kalman theory may be violated when a spacecraft maneuvers severely and the process noise might be heavy-tailed, which is prone to degrade IKF’s performance for attitude estimation. To address the attitude estimation problem with heavy-tailed process noise, this paper proposes a hierarchical Gaussian state-space model for invariant Kalman filtering: The probability density function of state prediction is defined based on student’s t distribution, while the conjugate prior distributions of the scale matrix and degrees of freedom (dofs) parameter are respectively formulated as the inverse Wishart and Gamma distribution. For the constructed hierarchical Gaussian attitude estimation state-space model, the Lie groups rotation matrix of spacecraft attitude is inferred together with the scale matrix and dof parameter using the variational Bayesian iteration. Numerical simulation results illustrate that the proposed approach can significantly improve the filtering robustness of invariant Kalman filter for Lie groups spacecraft attitude estimation problems with heavy-tailed process uncertainty.

1. Introduction

The navigation and control operations in aerospace engineering usually require adequate accurate knowledge of spacecraft orientation in space, and attitude determination from vector observations is usually employed in astronautically applications [1,2,3,4]. The widely used Kalman type filters can infer the state in Euclidean vector space by fusing attitude dynamic and observation sensor according to their probabilities [5,6,7,8]. Recently, the building of attitude dynamics on matrix Lie groups has been investigated actively for the navigation and control of spacecraft targets because it can significantly improve the estimation and control performance to make full use of the geometrical properties of Lie groups models [9,10].

For attitude estimation, there have already been significant research interests in the estimation and control of spacecraft targets such as the invariant observers and filters for attitude dynamics on the special orthogonal group SO(3), or the navigation systems defined on the special Euclidean group SE(3) [11,12]. Accounting for the geometrical invariance of symmetry attitude dynamics, the resulted attitude description with rotation matrix on SO(3) can avoid the trouble of singularities and unwinding encountered when using unit quaternion [13] and, to some extent, also contribute to better estimation performance for attitude determination algorithms [14]. Exploiting the mapping relation between matrix Lie groups and Lie algebra, invariant Kalman filter (IKF) can convert the attitude dynamics on SO(3) to Euclidean vector space and mimic classical Kalman filtering steps [15]. IKF is based on the invariance and log-linearity of symmetry dynamics, which then leads to better convergence on a much bigger set of state trajectories [16]. In these available researches, the process noise of attitude dynamical model was assumed to be a concentrated Gaussian distribution [17,18] and the statistics parameter are also assumed to be accurately given in advance as required in classical Kalman theory [19,20]. However, due to the presence of server maneuvers in space missions, there might be some unpredictable disturbances or outliers that would contaminate the Gaussian distribution and violate the required assumptions for Kalman filtering on matrix Lie groups.

For Euclidean space filtering problems, some adaptive methods are commonly used to deal with the trouble of inaccurate/unknown noise parameters [21,22,23]. Some adaptive techniques have been designed to estimate online unknown/inaccurate noise parameters, mainly including innovation-based adaptive methods and multiple model methods [24,25,26]; nevertheless, the convergence of innovation-based estimation methods cannot be guaranteed while the parallel-running multiple filter banks might increase the computational complexities. The variational Bayesian(VB) iteration-based adaptive methods have also been employed to jointly determine the system state and inaccurate noise statistics [27,28,29]. However, these adaptive methods all assume the noise distribution to be Gaussian and cannot be applied to the filtering cases with heavy-tailed noise. For heavy-tailed system noises, the Student’s t distribution assumes the distribution of the latent state and system noises as a joint Student’s t-distribution, and it has been proved to be more suitable to represent the non-Gaussian probability density function (PDF) [30,31].

To deal with the heavy-tailed process noise caused by severe maneuvering of rigid bodies, a robust Student’s t-based hierarchical Gaussian system model for the SINS/GPS integration is presented by modeling the prior probability density function as a Student’s t distribution and the conjugate prior distributions [21]. To improve the filtering performance of Student’s t-based methods for cases with both heavy-tailed process and observation noises, a robust Student’s t-based Kalman filter is proposed employing the variational Bayesian-based iterative approach in [32]; in the work of [33], by applying the Student’s t approximate distributions to posterior filtering and smoothing probability density functions, the robust nonlinear Student’s t-based filter and smoother are also presented as the generalization and extension of the linear Student’s t filtering methods. Note that the above researches provide a sound theoretical foundation for attitude estimation problems with heavy-tailed noises, but they are originally designed in Euclidean space systems for filtering of vector variable states. To the best of our knowledge, for attitude estimation problem in SO(3), until now there is still no special study on the robust Student’s t-based adaptive methods for invariant Kalman filtering on matrix Lie groups.

Note that although the available invariant Kalman filter also obeys the basic theory of classical Kalman filter, the above researches about robust Student’s t-based methods cannot be directly applied to matrix Lie groups systems. The uncertainty definition of concentrated Gaussian distribution on matrix Lie groups requires the assumption of noisy errors being small enough [34,35], which is the condition for first order approximation to the Baker–Campbell–Hausdorff (BCH) formula [36,37]. For attitude estimation on SO(3) in aerospace engineering, the presence of heavy-tailed noises and disturbances imposes a critical constraint on the application of robust Student’s t-based methods to improve the performance of invariant Kalman filter.

According to the above discussions, it is rather meaningful to investigate the robust Student’s t-based invariant Kalman filtering method especially for the matrix Lie groups attitude estimation problems in the presence of heavy-tailed outliers in system process noise. In this work, for attitude estimation problem with heavy-tailed process noise, this paper proposes a hierarchical Gaussian state-space model for invariant Kalman filtering, and the Lie groups rotation matrix of spacecraft attitude is inferred together with the scale matrix and dof parameter using the variational Bayesian iteration. Numerical simulation results illustrate that the proposed approach can significantly improve the filtering robustness of invariant Kalman filter for Lie groups spacecraft attitude estimation problems with heavy-tailed process uncertainty.

2. Primaries and Problem Definition

2.1. The Attitude Estimation System on the Special Orthogonal Group SO(3)

Attitude determinition from two vector observations is an important task in most aerospace engineering. The attitude dynamic on special orthogonal group $SO(3)\subset {ℝ}^{3\times 3}$ with the associated observation vectors can be formulated as the following system model [1,14]

$$R_k={\mathrm{Exp}}_G\left(w_k\right)R_{k-1}{\Omega }_{k-1}$$

(1)

$$Y_k=\left(\begin{array}{c}{y}_k^{’}\\ y_k^{’’}\end{array}\right)=\left(\begin{array}{c}{R}_k^T{b}^{’}+v_k^{’}\\ R_k^T{b}^{’’}+v_k^{’’}\end{array}\right),$$

(2)

where the special orthogonal group $SO(3):=\left\{R{R}^T=I_{3\times 3},\det \left(R\right)=1 \right\}$ is the group of 3 × 3 rotation matrices and $R_k\in SO(3)$ is the mapping rotation from the body frame of the spacecraft to the earth-fixed frame at time instant $k$; ${\Omega }_k\in SO(3)$ denotes the control rotation; $Y_k\in {ℝ}^6$ is a composition of the vector observations $y_k^{’},y_k^{’’}\in {ℝ}^3$ with their respective observation vector $b^{’},b^{’’}\in {ℝ}^3$;$v_k^{’}~{\mathcal{N}}^c\left(v_k^{’};0_{3\times 1},{\Sigma }_{v’}\right)$, and $v_k^{’’}~{\mathcal{N}}^c\left(v_k^{’};0_{3\times 1},{\Sigma }_{v^{″}}\right)$ are Gaussian white and mutually independent observation noises with ${\mathcal{N}}^c$ denoting the Gaussian distribution. Note that the vector observation based attitude determination is a common application scenario because the Sun sensor, magnetometer, horizon sensor, and the gravitational accelerometer are usually equipped by most spacecrafts in astronautical engineering [1]. A graphical presentation of target application scenario is given in Figure 1.

Note that the additive forms of vector noises in Euclidean space are not supported in Lie Groups space. The vector process noise $w_k\in {ℝ}^3$ is injected through Lie exponential operator ${\mathrm{Exp}}_G(\cdot )$ and, for any $w={\left[w_1 w_2 w_3\right]}^T\in {ℝ}^3$, the projection between the vector noise $w={\left[w_1 w_2 w_3\right]}^T\in {ℝ}^3$ and the associated groups element $R\in SO\left(3\right)$ are

$${\mathrm{Exp}}_G\left(\cdot \right):{ℝ}^3\to SO\left(3\right),{\mathrm{Exp}}_G^{-1}\left(\cdot \right):SO\left(3\right)\to {ℝ}^3$$

(3)

$${\mathrm{Exp}}_G\left(w\right):=\exp \left(w_{\times }\right)={\displaystyle \sum _{m=0}^{\infty }\frac{w_{\times }^m}{m!}}=I_{3\times 3}+\frac{\sin ‖w‖}{‖w‖}{w}_{\times }+2\frac{\sin {\left(‖w‖/2\right)}^2}{{‖w‖}^2}{w}_{\times }^2,$$

(4)

$$w_{\times }={\left(\begin{array}{c}{w}_1\\ w_2\\ w_3\end{array}\right)}_{\times }=\left(\begin{array}{ccc}0& -w_3& w_2\\ w_3& 0& -w_1\\ -w_2& w_1& 0\end{array}\right)\in {ℝ}^{3\times 3},$$

(5)

where $\exp (\cdot )$ is the matrix exponential operator and $w_{\times }$ is the skew-symmetric matrix. The concept of concentrated Gaussian distribution (CGD) was employed to define the probability distribution for random variables on matrix Lie groups [19,20,37]. For attitude estimation, in concentrated Gaussian distribution the groups variable $R\in SO\left(3\right)$ with mean $\overline{R}\in SO\left(3\right)$ and covariance $\Sigma \in {ℝ}^{3\times 3}$ is defined as

$$R~\mathcal{G}\left(R;\overline{R},\Sigma \right)\in SO\left(3\right) \mathrm{if} \xi \triangleq {\mathrm{Exp}}_G^{-1}\left(R{\overline{R}}^{-1}\right)~{\mathcal{N}}^c\left(\xi ;0_{3\times 1},\Sigma \right)\in {ℝ}^3$$

(6)

where $\mathcal{G}\left(\cdot ,\cdot \right)$ denotes the concentrated Gaussian distribution (CGD). Note that, in the definition of CGD, the eigenvalues of $\Sigma$ are assumed to be small enough, i.e., almost all the mass of the distribution should be concentrated in a small neighborhood around the mean of the rotation variable [20,37]. A graphical illustration of the relation between the concentrated Gaussian distribution on matrix Lie groups SO(3) and the classical Gaussian distribution relation is presented in Figure 2.

2.2. Model Projection Based on the Invariance Property of Attitude Estimation System

For attitude estimation on SO(3), the usual Kalman filtering methods in Euclidean vector space could not be applied directly. However, by properly defining the system variable, the Lie groups dynamic model (1) can be projected into the usual Euclidean space based on the invariance property. Let ${\widehat{R}}_{k-1}$ be an estimate for $R_{k-1}$ at instant k − 1 and ${\widehat{R}}_k={\widehat{R}}_{k-1}{\Omega }_{k-1}$ be the prediction for $R_k$. Define the invariant error ${\xi }_k={\mathrm{Exp}}_G^{-1}\left({\widehat{R}}_k{R}_k^{-1}\right)$, then the evolution model of invariant error ${\xi }_k$ obeys

$$\begin{array}{l}{\xi }_k={\mathrm{Exp}}_G^{-1}\left({\widehat{R}}_k{R}_k^{-1}\right)={\mathrm{Exp}}_G^{-1}\left({\widehat{R}}_{k-1}{\Omega }_{k-1}{\left({\mathrm{Exp}}_G\left(w_k\right)R_{k-1}{\Omega }_{k-1}\right)}^{-1}\right)\\ ={\mathrm{Exp}}_G^{-1}\left({\widehat{R}}_{k-1}{R}_{k-1}^{-1}{\mathrm{Exp}}_G\left(-w_k\right)\right)={\mathrm{Exp}}_G^{-1}\left({\mathrm{Exp}}_G\left({\xi }_{k-1}\right){\mathrm{Exp}}_G\left(-w_k\right)\right)\\ =\mathrm{BCH}\left({\xi }_{k-1},-w_k\right)={\xi }_{k-1}-w_k+O\left({\left|{\xi }_{k-1}\right|}^2,{\left|w_k\right|}^2,\left|{\xi }_{k-1}\right|\left|w_k\right|\right),\end{array}$$

(7)

where the $\mathrm{BCH}\left(\cdot ,\cdot \right)$ of the last step is a first order form of the Baker–Campbell–Hausdorff (BCH) formula [34,35,36]. Note that if the norm of the error term ${\xi }_k$ and $w_k$ are small enough so that the second order term in (7) could be negligible, then the matrix Lie groups attitude dynamic (1) for rotation matrix $R_k$ could be approximately converted into the following linear state space model in Euclidean space for the invariant error ${\xi }_k$

$${\xi }_k\doteq {\xi }_{k-1}-w_k.$$

(8)

As for observation model (2), the observation sequence $Y_k$ can also be reformulated into a function of the invariant error ${\xi }_k$, i.e., the new innovation sequence $z_k\in {ℝ}^6$ [14]

$$z_k={\widehat{R}}_k{Y}_k-\left(\begin{array}{c}{b}^{’}\\ b^{’’}\end{array}\right)=\left(\begin{array}{c}{\widehat{R}}_k{y}_k^{’}-b^{’}\\ {\widehat{R}}_k{y}_k^{’’}-b^{’’}\end{array}\right)={\widehat{R}}_k{R}_k^T\left(\begin{array}{c}{b}^{’}\\ b^{’’}\end{array}\right)-\left(\begin{array}{c}{b}^{’}\\ b^{’’}\end{array}\right)+{\widehat{R}}_{k|}\left(\begin{array}{c}{v}_k^{’}\\ v_k^{’’}\end{array}\right):=H_{\xi }{\xi }_k+V_k,$$

(9)

where $H=\left(\begin{array}{c}{b}_{\times }^{’}\\ b_{\times }^{’’}\end{array}\right) \in {ℝ}^{6\times 3}$ and $V_k={\widehat{R}}_{k|k-1}\left(\begin{array}{c}{v}_k^{’}\\ v_k^{’’}\end{array}\right)\in {ℝ}^6$ are the projected observation matrix and noise, respectively; note that, since the observation noises $v_k^{’},v_k^{’’}$ are isotropic and mutually independent noises and with ${\widehat{R}}_k{\widehat{R}}_k^T=I_{3\times 3}$, we have ${\Sigma }_V=\mathrm{Cov}\left(V_k{V}_k^T\right)=diag\left({\Sigma }_{v’},{\Sigma }_{v^{″}}\right)$.

Obviously, based on the definition of the invariant error ${\xi }_k$, the Lie groups dynamics (1) and observation model (2) could be converted as the Euclidean space model (8) and (9) for the invariant error ${\xi }_k$ (as in Figure 3). Note that both the true rotation state $R_{k-1}$ and the control rotation ${\Omega }_{k-1}$ do not appear in the converted model (8) and (9); therefore, the evolving motion of ${\xi }_k$ is actually independent of the true trajectory of rotation $R_{k-1}$ and control rotation ${\Omega }_{k-1}$, which is called the invariance property of symmetry model [15,16].

2.3. The Invariant Kalman Filter for Attitude Estimation

As the consequence of invariance property, for the attitude estimation system, the probability distribution of rotation state $R_k$ based on the observation $Y_k$ is equivalent to that of the invariant error ${\xi }_k$ in ${ℝ}^3$, constituting the basis of invariant Kalman filter. Then, if the process noise obeys classical Gaussian distribution, i.e., $w_k~{\mathcal{N}}^c\left(w_k;0_{3\times 1},{\Sigma }_w\right)\in {ℝ}^3$, the filtering steps for attitude estimation system on Lie group SO(3) can be implemented by mimicking that of the classical Kalman filter for invariant error ${\xi }_k$ in Euclidean space.

For the attitude dynamic (1) and associated observation (2), invariant Kalman filter (IKF) aims to recursively predict and correct the estimate for the rotation state. Given the initial estimate ${\widehat{R}}_{0|0}={\mathrm{Exp}}_G\left({\widehat{\xi }}_{0|0}\right)R_0$ for true $R_0$ with error ${\widehat{\xi }}_{0|0}\sim {\mathcal{N}}^c\left({\widehat{\xi }}_{0|0};0_{3\times 1},{\Sigma }_{0|0}\right)$, as the prediction for prior error estimate ${\widehat{\xi }}_{k|k-1}$ based on (8), the prior error rotation estimate ${\widehat{R}}_{k|k-1}$ based on the dynamic model (1) can be implemented as follows

$${\widehat{R}}_{k|k-1}={\widehat{R}}_{k-1|k-1}{\Omega }_{k-1} ,$$

(10)

where ${\widehat{R}}_{k|k-1}$ and ${\widehat{R}}_{k-1|k-1}$ are the prior and posterior error estimate for true rotation state $R_k$, respectively; ${\widehat{\xi }}_{k|k-1}$ and ${\widehat{\xi }}_{k-1|k-1}$ are associated with the prior and posterior error estimate for true error ${\xi }_k$. The covariance propagation of rotation group $R_k$ is same as that of the classical Kalman filter for invariant error ${\xi }_k$ based on the evolution model (8),

$${\Sigma }_{k|k-1}={\Sigma }_{k-1|k-1}+{\Sigma }_w,$$

(11)

where ${\Sigma }_{k-1|k-1}$ stands for the covariance of the posterior estimate ${\widehat{\xi }}_{k|k-1}$ and ${\widehat{R}}_{k|k-1}$. Therefore, based on the converted innovation (9), the correction steps of IKF for the rotation state ${\widehat{R}}_{k|k-1}$ can be implemented by updating the estimate ${\widehat{R}}_{k|k-1}$ with innovation sequence $z_k$

$${\widehat{R}}_{k|k}={\mathrm{Exp}}_G\left(K_k{z}_k\right){\widehat{R}}_{k|k-1},$$

(12)

$$K_k={\Sigma }_{k|k-1}{H}^T/\left(H{\Sigma }_{k|k-1}{H}^T+{\Sigma }_V\right),$$

(13)

$${\Sigma }_{k|k}={\Sigma }_{k|k-1}-K_{k}H{\Sigma }_{k|k-1},$$

(14)

where $K_k$ is the Kalman gain matrix, and the posterior estimate ${\widehat{R}}_{k|k}$ is then updated with Lie exponential operation of the correction term $K_k{z}_k$ as in (12) [16,17]. A graphical presentation of the filtering diagram of invariant Kalman filter is presented in Figure 4.

2.4. The Attitude Estimation Problem with the Trouble of Heavy-Tailed Process Noise

Obviously, the invariant Kalman filter (IKF) is a general extension of the Euclidean space Kalman filter to matrix Lie groups systems based on the invariance property of symmetry dynamics [16]. However, for the attitude estimation in aerospace engineering, there is still some difficulty for practical implementation of the IKF-based attitude estimation because the presence of heavy-tailed process noise in attitude dynamic might introduce great challenge and trouble to the theory of IKF:

Firstly, in the filtering step (10)~(14) of IKF, the process noise of attitude was assumed to be a concentrated Gaussian distribution and the statistics parameter is also required to be accurately known (as in Figure 4). However, due to the presence of severely maneuvering operations in space tasks, there might be some unpredictable disturbances or substantial outliers that would contaminate the Gaussian distribution and violate the assumptions for Kalman filtering on matrix Lie groups. Therefore, the theoretical foundation for optimal Kalman filtering is sure to be destroyed, which will degrade the final estimation.

Secondly, for the invariant projection of the Lie groups dynamics (1) into the linear vector form (8) in Section 2.2, the first order approximation to BCH formula is employed based on the assumption that both ${\xi }_k$ and $w_k$ were small enough and the second and higher order residual could be dropped from (7) to (8). However, for aerospace engineering applications, the unpredictable disturbances or substantial outliers caused by severely maneuvering operations might violate this condition and lead to inaccurate approximation to the system model, which is prone to a potential source of error and influences the estimation accuracy of IKF.

For attitude determination in astronautical engineering, it is rather meaningful to further study Lie groups estimation problems with heavy-tailed process noise and investigate new robust adaptive version of invariant Kalman filtering methods to improve the filtering precisions for cases with heavy-tailed process noises.

3. Robust Student’s t Based Invariant Kalman Filter for Attitude Estimation on SO(3)

3.1. Probability View of Attitude Estimation with Heavy-Tailed Process Noise

For attitude estimation, if the process noise $w_k$ in dynamic model (1) is heavy-tailed and contaminated by larger outliers, the Gaussian distribution and concentrated Gaussian distribution cannot accurately describe the probability distribution of heavy-tailed process noise. In this work, to account for the larger outliers due to severely maneuvering, the probability of the heavy-tailed noise is described as the student’s t distribution

$$p\left(w_k\right)={\mathcal{S}}_t\left(w_k;0_{3\times 1},{\Sigma }_w,{\tau }_k\right)={\displaystyle {\int }_0^{+\infty }{\mathcal{N}}^c\left(w_k;0_{3\times 1},{\Sigma }_w/{\gamma }_w\right)}\Gamma \left({\gamma }_w;\frac{{\tau }_w}{2},\frac{{\tau }_w}{2}\right)d{\gamma }_w$$

(15)

where $p\left(w_k\right)$ denotes the probability density of the process noise $w_k$; ${\mathcal{S}}_t\left(\cdot ;\mu ,\Sigma ,\tau \right)$ denotes the student’s t distribution with mean vector $\mu$, scale matrix $\Sigma$, and degree of parameter $\tau$; ${\gamma }_w$ is an auxiliary random variable in (15); and $\Gamma \left(\gamma ;\alpha ,\beta \right)$ represents the Gamma probability density function of the scalar $\gamma$ with $\alpha$ and $\beta$ as the shape and rate parameter, i.e.,

$$\Gamma \left(\gamma ;\alpha ,\beta \right)=\left\{\begin{array}{cc}\frac{{\beta }^{\alpha }}{\Upsilon \left(\alpha \right)}{\gamma }^{\alpha }{\mathrm{e}}^{\beta \gamma },& \gamma >0\\ 0,& \gamma \le 0\end{array}\right.$$

(16)

where $\mathrm{e}$ is the natural constant and $\mathsf{\Upsilon }\left(\cdot \right)$ represents the Gamma function. Obviously, the probability density function of the student’s t distribution can be regarded as the infinite mixture of the classical Gaussian probability density function [21,30].

If given the Gaussian approximation of the posterior probability density function $p\left(\left.{\xi }_{k-1}\right|z_{1:k-1}\right)$ and concentrated Gaussian approximation of posterior $p\left(\left.R_{k-1}\right|Y_{1:k-1}\right)$ for instant k−1, the prior probability density function of $p\left(\left.{\xi }_k\right|z_{1:k-1}\right)$ and $p\left(\left.R_k\right|Y_{1:k-1}\right)$ for time instant k can be obtained according to the Chapman–Kolmogorov equation [38,39],

$$p\left(\left.{\xi }_k\right|z_{1:k-1}\right)={\displaystyle \int p_w\left({\xi }_k-{\xi }_{k-1}\right)}p\left(\left.{\xi }_{k-1}\right|z_{1:k-1}\right)d{\xi }_{k-1}={\displaystyle \int p_w\left({\xi }_k-{\xi }_{k-1}\right)}{\mathcal{N}}^c\left({\xi }_{k-1};{\widehat{\xi }}_{\left.k-1\right|k-1},{\Sigma }_{\left.k-1\right|k-1}\right)d{\xi }_{k-1}$$

(17)

$$p\left(\left.R_k\right|Y_{1:k-1}\right)={\displaystyle \int p_w\left({\mathrm{Exp}}_G^{-1}\left(R_k{\Omega }_k^{-1}{R}_{k-1}\right)\right)}p\left(\left.R_{k-1}\right|Y_{1:k-1}\right)d{\xi }_{k-1}={\displaystyle \int p_w\left({\mathrm{Exp}}_G^{-1}\left(R_k{\Omega }_k^{-1}{R}_{k-1}\right)\right)}\mathcal{G}\left(R_{k-1};{\widehat{R}}_{\left.k-1\right|k-1},{\Sigma }_{\left.k-1\right|k-1}\right)d{R}_{k-1}$$

(18)

where $p\left(\left.{\xi }_{k-1}\right|z_{1:k-1}\right)$ denotes the posterior probability density function of the ${\xi }_{k-1}$ based on the innovation sequence $z_k$ from time instant 1 to k − 1 and $p\left(\left.R_{k-1}\right|Y_{1:k-1}\right)$ denotes the posterior probability density function of rotation $R_k$ based on observation $Y_k$ from instant 1 to k − 1. Note that resorting to the equivalence of the $R_k$ and ${\xi }_k$ in probability density function, $p_w\left({\xi }_k-{\xi }_{k-1}\right)=p_w\left({\mathrm{Exp}}_G^{-1}\left(R_k{\Omega }_k^{-1}{R}_{k-1}\right)\right)$ is the student’s t distribution $p\left(w_k\right)$ for system process noise and so we have $p\left(\left.R_k\right|Y_{1:k-1}\right)=p\left(\left.{\xi }_k\right|z_{1:k-1}\right)$. Further, it is evident that the prior probability $p\left(\left.{\xi }_k\right|z_{1:k-1}\right)$ for instant k is not Gaussian but a student’s t distribution with mean vector parameter ${\widehat{\xi }}_{k|k-1}$, scale matrix ${\Sigma }_{k|k-1}$ and parameter ${\tau }_k$, i.e.,

$$p\left(\left.R_k\right|Y_{1:k-1}\right)=p\left(\left.{\xi }_k\right|z_{1:k-1}\right)\triangleq {\mathcal{S}}_t\left({\xi }_k;{\widehat{\xi }}_{k|k-1},{\Sigma }_{k|k-1},{\tau }_k\right)={\displaystyle {\int }_0^{+\infty }{\mathcal{N}}^c\left({\xi }_k;{\widehat{\xi }}_{k|k-1},{\Sigma }_{k|k-1}/{\gamma }_k\right)}\Gamma \left({\gamma }_k;\frac{{\tau }_k}{2},\frac{{\tau }_k}{2}\right)d{\gamma }_k$$

(19)

where ${\widehat{\xi }}_{k|k-1}$, ${\Sigma }_{k|k-1}$, and ${\tau }_k$ are the mean vector, scale matrix, and degree of freedom parameter of the prior student’s t distribution $p\left(\left.R_k\right|Y_{1:k-1}\right)$; and ${\gamma }_k$ is the auxiliary random variable. Employing (15)~(19), the prior student’s t distribution $p\left(\left.R_k\right|Y_{1:k-1}\right)$ can be reformulated into a hierarchical composition of the Gaussian distributions

$$p\left(\left.{\xi }_k\right|{\gamma }_k,z_{1:k-1}\right)={\mathcal{N}}^c\left({\xi }_k;{\widehat{\xi }}_{\left.k\right|k-1},{\Sigma }_{\left.k\right|k-1}/{\gamma }_k\right),$$

(20)

$$p\left({\gamma }_k\right)=\Gamma \left({\gamma }_k;\frac{{\tau }_k}{2},\frac{{\tau }_k}{2}\right).$$

(21)

Attitude estimation aims to propagate the posterior probability density distribution based on the observation $Y_k$. Since the observation noise in (2) and its equivalent form $V_k$ in (9) are zero mean Gaussian white sequence with covariance ${\Sigma }_V$, the likelihood probability density function of the innovation sequence $z_k$ based on the error ${\xi }_k$ is equal to that of observation $Y_k$ given the rotation $R_k$, i.e.,

$$p\left(\left.Y_k\right|R_k\right)=p\left(\left.z_k\right|{\xi }_k\right)={\mathcal{N}}^c\left(z_k;H_{\xi }{\xi }_k,{\Sigma }_V\right)$$

(22)

where $p\left(\left.Y_k\right|R_k\right)$ is the probability density function of $Y_k$ given rotation $R_k$, and $p\left(\left.z_k\right|{\xi }_k\right)$ is the probability density function of $z_k$ based on error ${\xi }_k$.

Note that the accurate propagation of the probability density function requires accurate knowledge of scale matrix ${\Sigma }_{\left.k\right|k-1}$ and degree of freedom ${\tau }_k$. However, for real engineering applications in the presence of unpredictable disturbances or substantial outliers, these two distribution parameters are usually not correct or unavailable in advance. Therefore, from the view of probability density, the main troubles of attitude estimation with heavy-tailed process noise are threefold:

(1)

With the student’s t distribution process noise, the probability density function $p\left(\left.{\xi }_k\right|z_{1:k-1}\right)$ in classical invariant Kalman filter depends on the auxiliary random variable ${\gamma }_k$ and becomes the hierarchical form $p\left(\left.{\xi }_k\right|{\gamma }_k,z_{1:k-1}\right)$ in (20) and (21).

(2)

For the unpredictable disturbances or outliers induced by severely maneuvering operations, the accurate scale matrix ${\Sigma }_{\left.k\right|k-1}$ and degree of freedom ${\gamma }_k$ are usually unavailable but essential to propagate the posterior estimates.

As a consequence, the heavy-tailed attitude estimation problem cannot be solved with the invariant Kalman filter in Section 2 since the distribution parameter ${\Sigma }_{\left.k\right|k-1}$ and ${\tau }_k$ should be estimated together with the invariant error ${\xi }_k$ and the rotation state $R_k$.

3.2. Prior Probability Definition for the Parameters of Student’s t Distribution

To solve Lie groups filtering problems with Student’s t distribution process noise, this work aims to estimate the rotation group state $R_k$ together with ${\Sigma }_{\left.k\right|k-1}$ and ${\tau }_k$ by variational Bayesian iterations as in Figure 5. The prior distributions of parameter ${\Sigma }_{\left.k\right|k-1}$ and ${\tau }_k$ based on all historical observation sequences should be conjugate prior distributions so that their posterior distribution would maintain the same form as their prior distribution.

According to (20) and (21), the estimated scale matrix ${\Sigma }_{\left.k\right|k-1}$ is proportional to the covariance matrix of Gaussian distribution. Therefore, the commonly used inverse Wishart distribution and the Gamma distribution of Bayesian statistics are employed as the conjugate prior distributions for parameters ${\Sigma }_{\left.k\right|k-1}$ and ${\gamma }_k$, respectively.

The distribution of scale matrix ${\Sigma }_{\left.k\right|k-1}$ is defined as inverse Winshart distribution [29,39,40] which is often used as conjugate prior for the covariance matrix of the Gaussian variable. For attitude estimation problem, the inverse Winshart function of ${\Sigma }_{\left.k\right|k-1}$ can be written as

$$p\left({\Sigma }_{\left.k\right|k-1}\right)=\mathrm{IW}\left({\Sigma }_{\left.k\right|k-1};{\lambda }_k,{\mathsf{\Psi }}_k\right)=\frac{{\left|{\mathsf{\Psi }}_k\right|}^{{\lambda }_k/2}{\left|{\Sigma }_{\left.k\right|k-1}\right|}^{-({\lambda }_k+3+1)/2}}{2^{3{\lambda }_k/2}{\Upsilon }_3\left({\lambda }_k/2\right)}{\mathrm{e}}^{-0.5tr\left({\mathsf{\Psi }}_k{\Sigma }_{\left.k\right|k-1}^{-1}\right)},$$

(23)

where $\mathrm{IW}$ denotes the inverse Winshart distribution; ${\lambda }_k$ denotes the degrees of freedom; ${\mathsf{\Psi }}_k$ is the $3\times 3$ inverse scale matrix; $\left|\cdot \right|$ is the determinant operator; and ${\mathsf{\Upsilon }}_3\left(\cdot \right)$ is the 3-variate gamma function [29]. For the scale matrix ${\Sigma }_{\left.k\right|k-1}\sim \mathrm{IW}\left({\Sigma }_{\left.k\right|k-1};{\lambda }_k,{\mathsf{\Psi }}_k\right)$, the expectation of the matrix inverse is $E\left({\Sigma }_{\left.k\right|k-1}^{-1}\right)=\left({\lambda }_k-3-1\right){\mathsf{\Psi }}_k^{-1} \left(\mathrm{for} {\lambda }_k>3+1\right)$ and so we have

$$E\left({\Sigma }_{\left.k\right|k-1}\right)=\frac{{\mathsf{\Psi }}_k}{\left({\lambda }_k-3-1\right)}, \mathrm{if} \lambda >3+1.$$

(24)

The distribution parameter ${\tau }_k$ can be defined as the Gamma distribution [21],

$$p\left({\tau }_k\right)=\Gamma \left({\tau }_k;a_k,b_k\right),$$

(25)

where $a_k,b_k$ are respectively the shape and rate parameter of the Gamma distribution ${\tau }_k$, respectively.

In summary, Equations (20)~(25) actually constitute a new student’s t-based hierarchical Gaussian state-space model for the heavy-tailed attitude estimation problem, whose diagram is presented in Figure 6. The initialization of the required distribution parameter is based on the nominal parameter ${\Sigma }_{\left.k\right|k-1}$, which will be discussed later on. Using the above distribution definition, the rotation state $R_k$ and ${\xi }_k$, the auxiliary random variable ${\gamma }_k$, the scale matrix ${\Sigma }_{\left.k\right|k-1}$, and the parameter ${\tau }_k$ will be jointly estimated based on the newly constructed hierarchical Gaussian state-space model by variational Bayesian iterations, which then leads to the new robust student’s t-based invariant Kalman filter for the heavy-tailed attitude estimation problem.

3.3. Variational Beayesian Approximations of Posterior Probability Density Function

To jointly estimate the rotation state $R_k$, auxiliary random variable ${\gamma }_k$, scale matrix ${\Sigma }_{\left.k\right|k-1}$, and parameter ${\tau }_k$ requires calculating the posterior probability density function $p\left({\xi }_k,{\gamma }_k,{\Sigma }_{k|k-1},{\tau }_k\left|z_{1:k}\right.\right)$. Since the accurate analytical expression of $p\left({\xi }_k,{\gamma }_k,{\Sigma }_{k|k-1},{\tau }_k\left|z_{1:k}\right.\right)$ is not available, this work tries to obtain its factored approximate based on variational Bayesian iterations [38]. i.e.,

$$p\left({\xi }_k,{\gamma }_k,{\Sigma }_{k|k-1}{\tau }_k\left|z_{1:k}\right.\right)\approx q\left({\xi }_k\right)q\left({\gamma }_k\right)q\left({\Sigma }_{k|k-1}\right)q\left({\tau }_k\right),$$

(26)

where $q\left({\xi }_k\right)$, $q\left({\gamma }_k\right)$, $q\left({\Sigma }_{k|k-1}\right)$, and $q\left({\tau }_k\right)$ represent, respectively, the approximate posterior probability density function of ${\xi }_k$, ${\gamma }_k$, ${\Sigma }_{\left.k\right|k-1}$, ${\tau }_k$; $q\left({\xi }_k\right)$, $q\left({\gamma }_k\right)$, $q\left({\Sigma }_{k|k-1}\right)$ and $q\left({\tau }_k\right)$, which can be determined by minimizing the Kullback–Leibler divergence (KLD) [38,41] between the factored approximate $q\left({\xi }_k\right)q\left({\gamma }_k\right)q\left({\Sigma }_{k|k-1}\right)q\left({\tau }_k\right)$ and the true $p\left({\xi }_k,{\gamma }_k,{\Sigma }_{k|k-1},{\tau }_k\left|z_{1:k}\right.\right)$ [29,38], i.e.,

$$\left\{q\left({\xi }_k\right),q\left({\gamma }_k\right),q\left({\Sigma }_{k|k-1}\right),q\left({\tau }_k\right)\right\}=\arg \min \mathrm{KLD}\left(q\left({\xi }_k\right)q\left({\gamma }_k\right)q\left({\Sigma }_{k|k-1}\right)q\left({\tau }_k\right)‖p\left({\xi }_k,{\gamma }_k,{\Sigma }_{k|k-1},{\tau }_k\left|Y_{1:k}\right.\right)\right),$$

(27)

where $\mathrm{K}LD\left(q\left(x\right)‖p\left(x\right)\right)\triangleq {\displaystyle \int q\left(x\right)\log \frac{q\left(x\right)}{p\left(x\right)}dx}$ denotes the KLD between $q\left(x\right)$ and $p\left(x\right)$. According to the variational Bayesian iteration, these approximate posterior $q\left({\xi }_k\right)$, $q\left({\gamma }_k\right)$, $q\left({\Sigma }_{k|k-1}\right)$, and $q\left({\tau }_k\right)$ to (27) satisfy the following equations [21,38]

$$\log q\left({\xi }_k\right)={\mathrm{E}}_{-{\xi }_k}\left[\log p\left({\xi }_k,{\gamma }_k,{\Sigma }_{k|k-1},{\tau }_k,z_{1:k}\right)\right]+c_{{\xi }_k},$$

(28)

$$\log q\left({\gamma }_k\right)={\mathrm{E}}_{-{\gamma }_k}\left[\log p\left({\xi }_k,{\gamma }_k,{\Sigma }_{k|k-1},{\tau }_k,z_{1:k}\right)\right]+c_{{\gamma }_k},$$

(29)

$$\log q\left({\Sigma }_{\left.k\right|k-1}\right)={\mathrm{E}}_{-{\Sigma }_{\left.k\right|k-1}}\left[\log p\left({\xi }_k,{\gamma }_k,{\Sigma }_{k|k-1},{\tau }_k,z_{1:k}\right)\right]+c_{{\Sigma }_{\left.k\right|k-1}},$$

(30)

$$\log q\left({\tau }_k\right)={\mathrm{E}}_{-{\tau }_k}\left[\log p\left({\xi }_k,{\gamma }_k,{\Sigma }_{k|k-1},{\tau }_k,z_{1:k}\right)\right]+c_{{\tau }_k},$$

(31)

where $\log \left(\cdot \right)$ denotes the natural logarithm function; ${\mathrm{E}}_{-{\xi }_k}\left[\cdot \right]$ represents the mathematical expectation operation for all variables except ${\xi }_k$, while ${\mathrm{E}}_{-{\gamma }_k}\left[\cdot \right]$, ${\mathrm{E}}_{-{\Sigma }_{\left.k\right|k-1}}\left[\cdot \right]$, and ${\mathrm{E}}_{-{\tau }_k}\left[\cdot \right]$ are with similar definitions; $c_{{\xi }_k}$ is the constant with respect to ${\xi }_k$, while $c_{{\gamma }_k}$, $c_{{\Sigma }_{\left.k\right|k-1}}$, and $c_{{\tau }_k}$ are similarly defined. Obviously, Equations (28)~(31) are coupled and so no analytical solutions to the above equations exist. Therefore, only the approximate solutions are available and in this work the fixed-point iterations are employed to determine these parameters [38].

According to the conditional independence properties, the joint probability density function $p\left({\xi }_k,{\gamma }_k,{\Sigma }_{k|k-1},{\tau }_k,z_{1:k}\right)$ can be factored as follows

$$p\left({\xi }_k,{\gamma }_k,{\Sigma }_{k|k-1},{\tau }_k,z_{1:k}\right)=p\left(z_k\left|{\xi }_k\right.\right)p\left({\xi }_k\left|z_{1:k-1},{\gamma }_k\right.\right)p\left({\gamma }_k\right)p\left({\Sigma }_{k|k-1}\right)p\left({\tau }_k\right)p\left(z_{1:k-1}\right)$$

(32)

Then, using (20)~(26), we have

$$\begin{array}{l}p\left({\xi }_k,{\gamma }_k,{\Sigma }_{k|k-1},{\tau }_k,z_{1:k}\right) ={\mathcal{N}}^c\left(z_k;H{\xi }_k,{\Sigma }_V\right){\mathcal{N}}^c\left({\xi }_k;{\widehat{\xi }}_{\left.k\right|k-1},{\Sigma }_{\left.k\right|k-1}/{\gamma }_k\right)\\ \times \Gamma \left({\gamma }_k;\frac{{\tau }_k}{2},\frac{{\tau }_k}{2}\right)\mathrm{IW}\left({\Sigma }_{k|k-1};{\lambda }_k,{\mathsf{\Psi }}_k\right)\Gamma \left({\tau }_k;a_k,b_k\right)p\left(Y_{1:k-1}\right)\end{array}$$

(33)

Using (16) and (24), $\log p\left({\xi }_k,{\gamma }_k,{\Sigma }_{k|k-1},{\tau }_k,z_{1:k}\right)$ can be formulated as

$$\begin{array}{l}\log p\left({\xi }_k,{\gamma }_k,{\Sigma }_{k|k-1},{\tau }_k,z_{1:k}\right)=-\frac{1}{2}{\left(z_k-H{\xi }_k\right)}^T{\Sigma }_V^{-1}\left(z_k-H{\xi }_k\right)-\frac{1}{2}\left(5+{\lambda }_k\right)\log \left|{\Sigma }_{k|k-1}\right|-\frac{1}{2}\mathrm{tr}\left({\mathsf{\Psi }}_k{\Sigma }_{k|k-1}^{-1}\right)\\ -\frac{1}{2}{\gamma }_k{\left({\xi }_k-{\widehat{\xi }}_{\left.k\right|k-1}\right)}^T{\Sigma }_{k|k-1}^{-1}\left({\xi }_k-{\widehat{\xi }}_{\left.k\right|k-1}\right)+\frac{{\tau }_k+1}{2}\log {\gamma }_k-\frac{{\tau }_k{\gamma }_k}{2}+\frac{{\tau }_k}{2}\log \frac{{\tau }_k}{2}-\log \mathsf{\Upsilon }\left(\frac{{\tau }_k}{2}\right)\\ +\left(a_k-1\right)\log {\tau }_k-b_k{\tau }_k+\mathrm{constant}.\end{array}$$

(34)

3.4. Fixed-Point Iteration of the System State and Distribution Parameters

Based on the conditional independence property of the invariant error ${\xi }_k$ and parameters ${\gamma }_k$, ${\Sigma }_{\left.k\right|k-1}$, and ${\tau }_k$, the Equations (28)~(31) can be rewritten with above Equation (34) and fixed-point methods can be used to obtain the approximate estimates to the invariant error ${\xi }_k$ as well as the parameters ${\gamma }_k$,${\Sigma }_{\left.k\right|k-1}$, and ${\tau }_k$[21].

3.4.1. Fixed-Point Iteration of the Invariant Error ${\xi }_k$

With (34) and the independence property of ${\gamma }_k$ and ${\Sigma }_{\left.k\right|k-1}$, (28) can be written as

$$\begin{array}{l}\log q^{(i+1)}\left({\xi }_k\right)={\mathrm{E}}_{-{\xi }_k}^{(i+1)}\left[\log p({\xi }_k,{\gamma }_k,{\mathsf{\Sigma }}_{k\mid k-1},{\tau }_k,z_{1:k})\right]+c_{{\xi }_k}\\ =-\frac{1}{2}{(z_k-H{\xi }_k)}^T{\mathsf{\Sigma }}_V^{-1}(z_k-H{\xi }_k)-\frac{1}{2}{\mathrm{E}}_{-{\xi }_k}^{(i)}\left[{\gamma }_k{({\xi }_k-{\hat{\xi }}_{k\mid k-1})}^T{\mathsf{\Sigma }}_{k\mid k-1}^{-1}({\xi }_k-{\hat{\xi }}_{k\mid k-1})\right]+c_{{\xi }_k}\\ =-\frac{1}{2}{(z_k-H{\xi }_k)}^T{\mathsf{\Sigma }}_V^{-1}(z_k-H{\xi }_k)-\frac{1}{2}{\mathrm{E}}_{{\gamma }_k}^{(i)}\left[{\gamma }_k\right]{({\xi }_k-{\hat{\xi }}_{k\mid k-1})}^T{\mathrm{E}}_{{\mathsf{\Sigma }}_{kk1}}^{(i)}\left[{\mathsf{\Sigma }}_{k\mid k-1}^{-1}\right]({\xi }_k-{\hat{\xi }}_{k\mid k-1})+c_{{\xi }_k}\\ =-\frac{1}{2}{(z_k-H{\xi }_k)}^T{\mathsf{\Sigma }}_V^{-1}(z_k-H{\xi }_k)-\frac{1}{2}{({\xi }_k-{\hat{\xi }}_{k\mid k-1})}^T{\left({\overline{\mathsf{\Sigma }}}_{k\mid k-1}^{(i)}\right)}^{-1}({\xi }_k-{\hat{\xi }}_{k\mid k-1})+c_{{\xi }_k}\end{array}$$

(35)

where $q^{\left(i+1\right)}\left(\cdot \right)$ is the iterative approximation to $q\left(\cdot \right)$ at i + 1th iteration; ${\mathrm{E}}_{-{\xi }_k}^{\left(i\right)}\left[\cdot \right]$ represents the mathematical expectation at ith iteration for variables except the invariant error ${\xi }_k$ while ${\mathrm{E}}_{{\gamma }_k}^{\left(i\right)}\left[\cdot \right]$ and ${\mathrm{E}}_{{\Sigma }_{k|k-1}}^{\left(i\right)}\left[\cdot \right]$ denote the mathematical expectation at ith iteration for ${\gamma }_k$ and ${\Sigma }_{k|k-1}$, respectively; ${\stackrel{⌢}{\Sigma }}_{k|k-1}^{\left(i\right)}$ is the modified prior error covariance matrix calculated as

$${\stackrel{⌢}{\Sigma }}_{k|k-1}^{\left(i\right)}={\left\{{\mathrm{E}}_{{\Sigma }_{k|k-1}}^{\left(i\right)}\left[{\Sigma }_{k|k-1}^{-1}\right]\right\}}^{-1}/{\mathrm{E}}_{{\gamma }_k}^{\left(i\right)}\left[{\gamma }_k\right].$$

(36)

Then, $q^{\left(i+1\right)}\left({\xi }_k\right)$ can also be updated as a Gaussian distribution, i.e.,

$$q^{\left(i+1\right)}\left({\xi }_k\right)={\mathcal{N}}^c\left({\xi }_k;{\widehat{\xi }}_{k|k}^{\left(i+1\right)},{\Sigma }_{k|k}^{\left(i+1\right)}\right),$$

(37)

where ${\widehat{\xi }}_{k|k}^{\left(i+1\right)}$ and ${\Sigma }_{k|k}^{\left(i+1\right)}$ can be determined by following the update steps

$${\widehat{\xi }}_{k|k}^{\left(i+1\right)}={\widehat{\xi }}_{k|k-1}+K_k^{\left(i+1\right)}\left(z_k-H{\widehat{\xi }}_{k|k-1}\right),$$

(38)

$$K_k^{\left(i+1\right)}={\stackrel{⌢}{\Sigma }}_{k|k-1}^{\left(i\right)}{H}^T{\left(H{\stackrel{⌢}{\Sigma }}_{k|k-1}^{\left(i\right)}{H}^T+{\Sigma }_V\right)}^{-1},$$

(39)

$${\Sigma }_{k|k}^{\left(i+1\right)}={\stackrel{⌢}{\Sigma }}_{k|k-1}^{\left(i\right)}-K_{k|}^{\left(i+1\right)}H{\stackrel{⌢}{\Sigma }}_{k|k-1}^{\left(i\right)},$$

(40)

where $K_k^{\left(i+1\right)}$ denotes the calibrated Kalman gain matrix at the i + 1th iteration.

3.4.2. Fixed-Point Iteration of the Auxiliary Random Variable ${\gamma }_k$

Using (34) and conditional independency, the $\log q^{\left(i+1\right)}\left({\gamma }_k\right)$ in (29) can be rewritten as

$$\begin{array}{l}\log q^{\left(i+1\right)}\left({\gamma }_k\right)={\mathrm{E}}_{-{\gamma }_k}^{\left(i+1\right)}\left[\log p\left({\xi }_k,{\gamma }_k,{\Sigma }_{k|k-1},{\tau }_k,z_{1:k}\right)\right]+c_{{\gamma }_k}\\ =\frac{{\mathrm{E}}_{{\tau }_k}^{\left(i\right)}\left[{\tau }_k\right]+1}{2}\log {\gamma }_k-\frac{1}{2}\left\{{\mathrm{E}}_{-{\gamma }_k}^{\left(i\right)}\left[{\left({\xi }_k-{\widehat{\xi }}_{k|k-1}\right)}^T{\Sigma }_{k|k-1}^{-1}\left({\xi }_k-{\widehat{\xi }}_{k|k-1}\right)\right]+{\mathrm{E}}_{{\tau }_k}^{\left(i\right)}\left[{\tau }_k\right]\right\}{\gamma }_k+c_{{\gamma }_k}\\ =\frac{{\mathrm{E}}_{{\tau }_k}^{\left(i\right)}\left[{\tau }_k\right]+1}{2}\log {\gamma }_k-\frac{1}{2}\left\{tr\left({\mathrm{E}}_{{\xi }_k}^{\left(i\right)}\left[\left({\xi }_k-{\widehat{\xi }}_{k|k-1}\right){\left({\xi }_k-{\widehat{\xi }}_{k|k-1}\right)}^T\right]{\mathrm{E}}_{{\Sigma }_{k|k-1}}^{\left(i\right)}\left[{\Sigma }_{k|k-1}^{-1}\right]\right)+{\mathrm{E}}_{{\tau }_k}^{\left(i\right)}\left[{\tau }_k\right]\right\}{\gamma }_k+c_{{\gamma }_k}.\end{array}$$

(41)

where ${\mathrm{E}}_{-{\gamma }_k}^{\left(i\right)}\left[\cdot \right]$ represents the mathematical expectation at ith iteration for variables except the ${\gamma }_k$ while ${\mathrm{E}}_{{\tau }_k}^{\left(i\right)}\left[\cdot \right]$ denotes the mathematical expectation at ith iteration for the parameter ${\tau }_k$. Note that the approximate ${\widehat{\xi }}_{k|k}^{\left(i+1\right)}$ and ${\Sigma }_{k|k}^{\left(i+1\right)}$ updated in (35)~(40) can be used to propagate Equation (41) and the term ${\mathrm{E}}_{{\xi }_k}^{\left(i\right)}\left[\left({\xi }_k-{\widehat{\xi }}_{k|k-1}\right){\left({\xi }_k-{\widehat{\xi }}_{k|k-1}\right)}^T\right]$ can be reformulated as

$$\begin{gathered} \begin{array}{l}{\mathrm{E}}_{-{\gamma }_k}^{\left(i\right)}\left[\left({\xi }_k-{\widehat{\xi }}_{k|k-1}\right){\left({\xi }_k-{\widehat{\xi }}_{k|k-1}\right)}^T\right]={\mathrm{E}}_{-{\gamma }_k}^{\left(i\right)}\left[\left({\xi }_k-{\widehat{\xi }}_{k|k}^{\left(i+1\right)}+{\widehat{\xi }}_{k|k}^{\left(i+1\right)}-{\widehat{\xi }}_{k|k-1}\right)\right.\left.{\left({\xi }_k-{\widehat{\xi }}_{k|k}^{\left(i+1\right)}+{\widehat{\xi }}_{k|k}^{\left(i+1\right)}-{\widehat{\xi }}_{k|k-1}\right)}^T\right]\end{array} \\ ={\mathrm{E}}_{-{\gamma }_k}^{\left(i+1\right)}\left[\left({\xi }_k-{\widehat{\xi }}_{k|k}^{\left(i+1\right)}\right){\left({\xi }_k-{\widehat{\xi }}_{k|k}^{\left(i+1\right)}\right)}^T\right]+\left({\widehat{\xi }}_{k|k}^{\left(i+1\right)}-{\widehat{\xi }}_{k|k-1}\right){\left({\widehat{\xi }}_{k|k}^{\left(i+1\right)}-{\widehat{\xi }}_{k|k-1}\right)}^T={\Sigma }_{k|k}^{\left(i+1\right)}+\left({\widehat{\xi }}_{k|k}^{\left(i+1\right)}-{\widehat{\xi }}_{k|k-1}\right){\left({\widehat{\xi }}_{k|k}^{\left(i+1\right)}-{\widehat{\xi }}_{k|k-1}\right)}^T \end{gathered}$$

(42)

Then (41) could be deduced into

$$\begin{array}{l}\log q^{\left(i+1\right)}\left({\gamma }_k\right)=\left(\frac{{\mathrm{E}}_{{\tau }_k}^{\left(i\right)}\left[{\tau }_k\right]+3}{2}-1\right)\log {\gamma }_k\\ -\frac{1}{2}\left\{tr\left({\Sigma }_{k|k}^{\left(i+1\right)}{\mathrm{E}}_{{\Sigma }_{k|k-1}}^{\left(i\right)}\left[{\Sigma }_{k|k-1}^{-1}\right]+\left({\widehat{\xi }}_{k|k}^{\left(i+1\right)}-{\widehat{\xi }}_{k|k-1}\right){\left({\widehat{\xi }}_{k|k}^{\left(i+1\right)}-{\widehat{\xi }}_{k|k-1}\right)}^T{\mathrm{E}}_{{\Sigma }_{k|k-1}}^{\left(i\right)}\left[{\Sigma }_{k|k-1}^{-1}\right]\right)+{\mathrm{E}}_{{\tau }_k}^{\left(i\right)}\left[{\tau }_k\right]\right\}{\gamma }_k+c_{{\gamma }_k}.\end{array}$$

(43)

which is actually also a Gamma distribution as (16), i.e.,

$$q^{\left(i+1\right)}\left({\gamma }_k\right)=\Gamma \left({\gamma }_k;{\eta }^{\left(i+1\right)},{\theta }^{\left(i+1\right)}\right),$$

(44)

where ${\eta }^{\left(i+1\right)}$ and ${\theta }^{\left(i+1\right)}$ are the shape and rate parameter determined by

$${\eta }^{\left(i+1\right)}=\frac{{\mathrm{E}}_{{\tau }_k}^{\left(i\right)}\left[{\tau }_k\right]+3}{2},$$

(45)

$${\theta }^{\left(i+1\right)}=\frac{1}{2}\left\{tr\left(\left({\Sigma }_{k|k}^{\left(i+1\right)}+U_k^{\left(i\right)}+\left({\widehat{\xi }}_{k|k}^{\left(i+1\right)}-{\widehat{u}}_k^{\left(i\right)}\right){\left({\widehat{\xi }}_{k|k}^{\left(i+1\right)}-{\widehat{u}}_k^{\left(i\right)}\right)}^T\right){\mathrm{E}}_{{\Sigma }_{k|k-1}}^{\left(i\right)}\left[{\Sigma }_{k|k-1}^{-1}\right]\right)+{\mathrm{E}}_{{\tau }_k}^{\left(i\right)}\left[{\tau }_k\right]\right\}.$$

(46)

3.4.3. Fixed-Point Iteration of the Prior Estimate for Covariance Matrix ${\Sigma }_{k|k-1}$

With (34), (37), (44) and the independence of ${\xi }_k$ and ${\gamma }_k$, (30) can be rewritten as

$$\begin{array}{l}\log q^{\left(i+1\right)}\left({\Sigma }_{k|k-1}\right)={\mathrm{E}}_{-{\Sigma }_{\left.k\right|k-1}}^{\left(i+1\right)}\left[\log p\left({\xi }_k,{\gamma }_k,{\Sigma }_{k|k-1},{\tau }_k,z_{1:k}\right)\right]+c_{{\Sigma }_{k|k-1}}\\ =-\frac{1}{2}{\mathrm{E}}_{-{\Sigma }_{\left.k\right|k-1}}^{\left(i\right)}\left[{\gamma }_k{\left({\xi }_k-{\widehat{\xi }}_{k|k-1}\right)}^T{\Sigma }_{k|k-1}^{-1}\left({\xi }_k-{\widehat{\xi }}_{k|k-1}\right)\right]-\frac{1}{2}{\left(5+{\lambda }_k\right)}^T\log \left|{\Sigma }_{k|k-1}\right|-\frac{1}{2}tr\left({\mathsf{\Psi }}_k{\Sigma }_{\left.k\right|k-1}^{-1}\right)+c_{{\Sigma }_{k|k-1}}\\ =-\frac{1}{2}{\left(5+{\lambda }_k\right)}^T\log \left|{\Sigma }_{k|k-1}\right|-\frac{1}{2}tr\left\{\left({\mathsf{\Psi }}_k+{\mathrm{E}}_{{\xi }_k}^{\left(i+1\right)}\left[{\xi }_k\right]B_k^{\left(i+1\right)}\right){\Sigma }_{\left.k\right|k-1}^{-1}\right\}+c_{{\Sigma }_{k|k-1}}\end{array}$$

(47)

where the term $B_k^{\left(i+1\right)}$ is calculated as

$$\begin{array}{l}{B}_k^{\left(i+1\right)}={\mathrm{E}}_{-{\Sigma }_{\left.k\right|k-1}}^{\left(i+1\right)}\left[\left({\xi }_k-{\widehat{\xi }}_{k|k-1}\right){\left({\xi }_k-{\widehat{\xi }}_{k|k-1}\right)}^T\right]\\ ={\mathrm{E}}_{-{\Sigma }_{\left.k\right|k-1}}^{\left(i+1\right)}\left[\left({\xi }_k-{\widehat{\xi }}_{k|k}^{\left(i+1\right)}+{\widehat{\xi }}_{k|k}^{\left(i+1\right)}-{\widehat{\xi }}_{k|k-1}\right)\right.\left.{\left({\xi }_k-{\widehat{\xi }}_{k|k}^{\left(i+1\right)}+{\widehat{\xi }}_{k|k}^{\left(i+1\right)}-{\widehat{\xi }}_{k|k-1}\right)}^T\right]\\ ={\Sigma }_{k|k}^{\left(i+1\right)}+\left({\widehat{\xi }}_{k|k}^{\left(i+1\right)}-{\widehat{\xi }}_{k|k-1}\right){\left({\widehat{\xi }}_{k|k}^{\left(i+1\right)}-{\widehat{\xi }}_{k|k-1}\right)}^T,\end{array}$$

(48)

Therefore, using (24) and (47), $q^{\left(i+1\right)}\left({\Sigma }_{k|k-1}\right)$ is also an inverse Wishart distribution as

$$q^{\left(i+1\right)}\left({\Sigma }_{k|k-1}\right)=\mathrm{IW}\left({\Sigma }_{k|k-1};{\widehat{\lambda }}_k^{\left(i+1\right)},{\widehat{\mathsf{\Psi }}}_k^{\left(i+1\right)}\right),$$

(49)

where ${\widehat{\lambda }}_k^{\left(i+1\right)},{\widehat{\mathsf{\Psi }}}_k^{\left(i+1\right)}$ are given as

$${\widehat{\lambda }}_k^{\left(i+1\right)}={\lambda }_k+1,$$

(50)

$${\widehat{\mathsf{\Psi }}}_k^{\left(i+1\right)}={\mathsf{\Psi }}_k+{\mathrm{E}}_{{\xi }_k}^{\left(i+1\right)}\left[{\xi }_k\right]B_k^{\left(i+1\right)}.$$

(51)

3.4.4. Fixed-Point Iteration of the Prior Estimate for Parameter ${\tau }_k$

With (34), (37), (44), and (49), (33) can be rewritten as

$$\begin{array}{l}\log q^{\left(i+1\right)}\left({\tau }_k\right)={\mathrm{E}}_{-{\tau }_k}^{\left(i+1\right)}\left[\log p\left({\xi }_k,{\gamma }_k,{\Sigma }_{k|k-1},{\tau }_k,z_{1:k}\right)\right]+c_{{\tau }_k}\\ =\frac{{\tau }_k}{2}{\mathrm{E}}_{{\gamma }_k}^{\left(i+1\right)}\left[\log {\gamma }_k\right]-\frac{1}{2}{\mathrm{E}}_{{\gamma }_k}^{\left(i+1\right)}\left[{\gamma }_k\right]{\tau }_k+\frac{{\tau }_k}{2}\log \frac{{\tau }_k}{2}-\log \mathsf{\Upsilon }\left(\frac{{\tau }_k}{2}\right)+\left(a_k-1\right)\log {\tau }_k-b_k{\tau }_k+c_{{\tau }_k}.\end{array}$$

(52)

With Stirling’s equation $\log \mathsf{\Upsilon }\left(\frac{{\tau }_k}{2}\right)\approx \left(\frac{{\tau }_k-1}{2}\right)\log \left(\frac{{\tau }_k}{2}\right)-\frac{{\tau }_k}{2}$, (52) can be approximated as

$$\log q^{\left(i+1\right)}\left({\tau }_k\right)=\left(a_k+\frac{1}{2}-1\right)\log {\tau }_k-\left(b_k-\frac{1}{2}-\frac{1}{2}{\mathrm{E}}_{{\gamma }_k}^{\left(i+1\right)}\left[\log {\gamma }_k\right]+\frac{1}{2}{\mathrm{E}}_{{\gamma }_k}^{\left(i+1\right)}\left[{\gamma }_k\right]\right){\tau }_k+c_{{\tau }_k}.$$

(53)

Therefore, using (16) and (53), $q^{\left(i+1\right)}\left({\tau }_k\right)$ can be updated as a Gamma distribution, i.e.,

$$q^{\left(i+1\right)}\left({\tau }_k\right)=\Gamma \left({\tau }_k;{\widehat{a}}_k^{\left(i+1\right)},{\widehat{b}}_k^{\left(i+1\right)}\right),$$

(54)

where ${\widehat{a}}_k^{\left(i+1\right)},{\widehat{b}}_k^{\left(i+1\right)}$ are given as

$${\widehat{a}}_k^{\left(i+1\right)}=a_k+\frac{1}{2},$$

(55)

$${\widehat{b}}_k^{\left(i+1\right)}=b_k-\frac{1}{2}-\frac{1}{2}{\mathrm{E}}_{{\gamma }_k}^{\left(i+1\right)}\left[\log {\gamma }_k\right]+\frac{1}{2}{\mathrm{E}}_{{\gamma }_k}^{\left(i+1\right)}\left[{\gamma }_k\right].$$

(56)

3.5. The Variational Beayesian Iteration Based Robust Student’s t Invariant Kalman Filter

To propagate the approximate posterior distributions, using (44), (49) and (54), the expectations ${\mathrm{E}}_{{\gamma }_k}^{\left(i+1\right)}\left[\log {\gamma }_k\right]$, ${\mathrm{E}}_{{\gamma }_k}^{\left(i+1\right)}\left[{\gamma }_k\right]$, ${\mathrm{E}}_{{\Sigma }_{k|k-1}}^{\left(i+1\right)}\left[{\Sigma }_{k|k-1}^{-1}\right]$, and ${\mathrm{E}}_{{\tau }_k}^{\left(i+1\right)}\left[{\tau }_k\right]$ can be calculated as follows

$${\mathrm{E}}_{{\gamma }_k}^{\left(i+1\right)}\left[{\gamma }_k\right]={\eta }^{\left(i+1\right)}/{\theta }^{\left(i+1\right)},$$

(57)

$${\mathrm{E}}_{{\gamma }_k}^{\left(i+1\right)}\left[\log {\gamma }_k\right]=\psi \left({\eta }^{\left(i+1\right)}\right)-\log {\theta }^{\left(i+1\right)},$$

(58)

$${\mathrm{E}}_{{\Sigma }_{k|k-1}}^{\left(i+1\right)}\left[{\Sigma }_{k|k-1}^{-1}\right]=\left({\widehat{\lambda }}_k^{\left(i+1\right)}-4\right){\left({\widehat{\mathsf{\Psi }}}_k^{\left(i+1\right)}\right)}^{-1},$$

(59)

$${\mathrm{E}}_{{\tau }_k}^{\left(i+1\right)}\left[{\tau }_k\right]={\widehat{a}}_k^{\left(i+1\right)}/{\widehat{b}}_k^{\left(i+1\right)},$$

(60)

where $\psi \left(\cdot \right)$ represents a digamma function [42]. After N times of fixed-point iterations, the required posterior probability density function of ${\xi }_k$ could be approximated as

$$q\left({\xi }_k\right)\approx {\mathcal{N}}^c\left({\xi }_k;{\widehat{\xi }}_{k|k}^{\left(N\right)},{\Sigma }_{k|k}^{\left(N\right)}\right)={\mathcal{N}}^c\left({\xi }_k;{\widehat{\xi }}_{k|k},{\Sigma }_{k|k}\right),$$

(61)

where ${\widehat{\xi }}_{k|k},{\Sigma }_{k|k}$ are respectively the posterior estimate for invariant error ${\xi }_k$ and the posterior estimate error covariance parameter. Then, resorting to the equivalence of probability distribution, the distributing of the propagated rotation state $R_k$ can be updated as the concentrated Gaussian distribution with mean being ${\widehat{R}}_{k|k}$ and covariance being ${\Sigma }_{k|k}$,i.e.,

$$q\left(R_k\right)=\mathcal{G}\left(R_k;{\widehat{R}}_{k|k},{\Sigma }_{k|k}\right),$$

(62)

$${\widehat{R}}_{k|k}=\exp \left({\left[K_k^{\left(N\right)}\left(z_k-H{\widehat{\xi }}_{k|k-1}\right)\right]}^{\wedge }\right){\widehat{R}}_{k|k-1}=\exp \left({\left[K_k^{\left(N\right)}\left(z_k-H{\widehat{\xi }}_{k|k-1}\right)\right]}^{\wedge }\right){\widehat{R}}_{k-1|k-1}{\Omega }_{k-1}$$

(63)

where the iterative Kalman gain matrix $K_k^{\left(N\right)}$ is calculated in (39). The Equations (62) and (63) are actually the projection of the Euclidean space Equation (38) into Lie group space.

Therefore, the filtering steps of the proposed approach include the steps (36)~(40), (42)~(46), (48)~(51), and (54)~(63). The detailed implementation of the proposed method for attitude estimation is presented in Algorithm 1. For the heavy-tailed attitude estimation, the nominal ${\tilde{\Sigma }}_{k|k-1}$ could be selected as (11) to initialize the filter parameter ${\mathsf{\Psi }}_k$ and ${\lambda }_k$; the invariant error is defined as the Lie exponential error of ${\widehat{R}}_{\left.k-1\right|k-1}$ and $R_k$ so it could be assumed to be zero at the initialization to propagate the prior estimate ${\widehat{\xi }}_{k|k-1}={\widehat{\xi }}_{k-1|k-1}$.

In invariant Kalman filter, the prior error estimate ${\widehat{R}}_{k|k-1}$ is actually based on all the historical observations $Y_{1:k-1}$ before time instant k. In this work, the covariance parameter ${\Sigma }_{k|k-1}$ is also inferred using the historical observations $Y_{1:k-1}$ until time instant k. As for the inverse Wishart distribution of ${\Sigma }_{k|k-1}$, at time instant k the degree of parameter is set as ${\lambda }_k=k+4$ and the inverse scale matrix parameter is set as ${\mathsf{\Psi }}_k=k{\tilde{\Sigma }}_{k|k-1}$.

In practical numerical integration of the attitude dynamic model, if there exists severe maneuvering, the system process noise will have a heavy-tailed distribution and the degree of freedom parameter ${\tau }_k$ depends on the maneuvering intensity. According to our experience, the initial estimates for the shape parameter $a_k$ and rate parameter $b_k$ could be set as $a_k/b_k\in \left[3,10\right]$ for the iteratively updating ${\widehat{a}}_k,{\widehat{b}}_k$ in Algorithm 1.

Algorithm 1. The filtering steps of one time instant in the proposed approach for attitude estimation.

Inputs:${\widehat{R}}_{k-1|k-1}$,${\Sigma }_{k-1|k-1}$,${\widehat{\xi }}_{k-1|k-1}$,${\Omega }_{k-1}$,$H$,$Y_k$,${\Sigma }_w$,${\Sigma }_V$,${\lambda }_k,{\mathsf{\Psi }}_k,a_k,b_k,N$ 1. Predict the nominal invariant error ${\widehat{\xi }}_{k|k-1}$ and rotation ${\widehat{R}}_{k|k-1}$ according to (8) and (10) ${\widehat{\xi }}_{k|k-1}={\widehat{\xi }}_{k-1|k-1}$,${\widehat{R}}_{k|k-1}={\widehat{R}}_{k-1|k-1}{\Omega }_{k-1}$ 2. Predict the nominal prior error covariance ${\tilde{\Sigma }}_{k|k-1}$ according to (11) ${\tilde{\Sigma }}_{k|k-1}={\Sigma }_{k-1|k-1}+{\Sigma }_w$ 3. Calculate the converted innovation according to (9) $z_k={\widehat{R}}_{k|k-1}{Y}_k-\left(\begin{array}{c}{b}^{’}\\ b^{’’}\end{array}\right)$ 4. Initialize the prior parameters ${\lambda }_k^{\left(0\right)}={\lambda }_k$,${\widehat{\mathsf{\Psi }}}_k^{\left(0\right)}={\mathsf{\Psi }}_k=\left({\lambda }_k-4\right){\tilde{\Sigma }}_{k|k-1}$,${\mathrm{E}}_{{\gamma }_k}^{\left(0\right)}\left[{\gamma }_k\right]=1$ 5. Calculate initial expectations according to (24) and (25) ${\mathrm{E}}_{{\Sigma }_{k|k-1}}^{\left(0\right)}\left[{\Sigma }_{k|k-1}^{-1}\right]=\left({\widehat{\lambda }}_k^{\left(0\right)}-4\right){\left({\widehat{\mathsf{\Psi }}}_k^{\left(0\right)}\right)}^{-1}$,${\mathrm{E}}_{{\tau }_k}^{\left(0\right)}\left[{\tau }_k\right]=a_k/b_k$ for i = 0:N − 1 6. Update $q^{\left(i+1\right)}\left({\xi }_k\right)={\mathcal{N}}^c\left({\xi }_k;{\widehat{\xi }}_{k|k}^{\left(i+1\right)},{\Sigma }_{k|k}^{\left(i+1\right)}\right)$ according to (36)~(40) ${\stackrel{⌢}{\Sigma }}_{k|k-1}^{\left(i\right)}={\left\{{\mathrm{E}}_{{\Sigma }_{k|k-1}}^{\left(i\right)}\left[{\Sigma }_{k|k-1}^{-1}\right]\right\}}^{-1}/{\mathrm{E}}_{{\gamma }_k}^{\left(i\right)}\left[{\gamma }_k\right]$,$K_k^{\left(i+1\right)}={\stackrel{⌢}{\Sigma }}_{k|k-1}^{\left(i\right)}{H}^T{\left(H{\stackrel{⌢}{\Sigma }}_{k|k-1}^{\left(i\right)}{H}^T+{\Sigma }_V\right)}^{-1}$, ${\Sigma }_{k|k}^{\left(i+1\right)}={\stackrel{⌢}{\Sigma }}_{k|k-1}^{\left(i\right)}-K_{k|}^{\left(i+1\right)}H{\stackrel{⌢}{\Sigma }}_{k|k-1}^{\left(i\right)}$,${\widehat{\xi }}_{k|k}^{\left(i+1\right)}={\widehat{\xi }}_{k|k-1}+K_k^{\left(i+1\right)}\left(z_k-H{\widehat{\xi }}_{k|k-1}\right)$ 7. Update $q^{\left(i+1\right)}\left({\gamma }_k\right)=\Gamma \left({\gamma }_k;{\eta }^{\left(i+1\right)},{\theta }^{\left(i+1\right)}\right)$according to (44)~(46) ${\eta }^{\left(i+1\right)}=\frac{{\mathrm{E}}_{{\tau }_k}^{\left(i\right)}\left[{\tau }_k\right]+3}{2}$, ${\theta }^{\left(i+1\right)}=\frac{1}{2}\left\{tr\left(\left({\Sigma }_{k|k}^{\left(i+1\right)}+\left({\widehat{\xi }}_{k|k}^{\left(i+1\right)}-{\widehat{\xi }}_{k|k-1}\right){\left({\widehat{\xi }}_{k|k}^{\left(i+1\right)}-{\widehat{\xi }}_{k|k-1}\right)}^T\right){\mathrm{E}}_{{\Sigma }_{k|k-1}}^{\left(i\right)}\left[{\Sigma }_{k|k-1}^{-1}\right]\right)+{\mathrm{E}}_{{\tau }_k}^{\left(i\right)}\left[{\tau }_k\right]\right\}$ 8. Update $q^{\left(i+1\right)}\left({\Sigma }_{k|k-1}\right)=\mathrm{IW}\left({\Sigma }_{k|k-1};{\widehat{\lambda }}_k^{\left(i+1\right)},{\widehat{\mathsf{\Psi }}}_k^{\left(i+1\right)}\right)$according to (48)~(51) $B_k^{\left(i+1\right)}={\Sigma }_{k|k}^{\left(i+1\right)}+\left({\widehat{\xi }}_{k|k}^{\left(i+1\right)}-{\widehat{\xi }}_{k|k-1}\right){\left({\widehat{\xi }}_{k|k}^{\left(i+1\right)}-{\widehat{\xi }}_{k|k-1}\right)}^T$,${\widehat{\lambda }}_k^{\left(i+1\right)}={\lambda }_k+1$,${\widehat{\mathsf{\Psi }}}_k^{\left(i+1\right)}={\mathsf{\Psi }}_k+{\mathrm{E}}_{{\xi }_k}^{\left(i+1\right)}\left[{\xi }_k\right]B_k^{\left(i+1\right)}$ 9. Update expectations ${\mathrm{E}}_{{\gamma }_k}^{\left(i+1\right)}\left[\log {\gamma }_k\right]$,${\mathrm{E}}_{{\gamma }_k}^{\left(i+1\right)}\left[{\gamma }_k\right]$ and ${\mathrm{E}}_{{\Sigma }_{k|k-1}}^{\left(i+1\right)}\left[{\Sigma }_{k|k-1}^{-1}\right]$ according to (57)~(59) ${\mathrm{E}}_{{\gamma }_k}^{\left(i+1\right)}\left[{\gamma }_k\right]={\eta }^{\left(i+1\right)}/{\theta }^{\left(i+1\right)}$,${\mathrm{E}}_{{\gamma }_k}^{\left(i+1\right)}\left[\log {\gamma }_k\right]=\psi \left({\eta }^{\left(i+1\right)}\right)-\log {\theta }^{\left(i+1\right)}$, ${\mathrm{E}}_{{\Sigma }_{k|k-1}}^{\left(i+1\right)}\left[{\Sigma }_{k|k-1}^{-1}\right]=\left({\widehat{\lambda }}_k^{\left(i+1\right)}-4\right){\left({\widehat{\mathsf{\Psi }}}_k^{\left(i+1\right)}\right)}^{-1}$ 10. Update $q^{\left(i+1\right)}\left({\tau }_k\right)=\Gamma \left({\tau }_k;{\widehat{a}}_k^{\left(i+1\right)},{\widehat{b}}_k^{\left(i+1\right)}\right)$according to (55) and (56) ${\widehat{a}}_k^{\left(i+1\right)}=a_k+\frac{1}{2}$,${\widehat{b}}_k^{\left(i+1\right)}=b_k-\frac{1}{2}-\frac{1}{2}{\mathrm{E}}_{{\gamma }_k}^{\left(i+1\right)}\left[\log {\gamma }_k\right]+\frac{1}{2}{\mathrm{E}}_{{\gamma }_k}^{\left(i+1\right)}\left[{\gamma }_k\right]$ 11. Calculate the expectation ${\mathrm{E}}_{{\tau }_k}^{\left(i+1\right)}\left[{\tau }_k\right]$ according to (60) ${\mathrm{E}}_{{\tau }_k}^{\left(i+1\right)}\left[{\tau }_k\right]={\widehat{a}}_k^{\left(i+1\right)}/{\widehat{b}}_k^{\left(i+1\right)}$ end for 12. Update the posterior rotation group and its covariance according to (63) ${\widehat{R}}_{k|k}=\exp \left({\left[K_k^{\left(N\right)}\left(z_k-H{\widehat{\xi }}_{k|k-1}\right)\right]}^{\wedge }\right){\widehat{R}}_{k|k-1}$,${\Sigma }_{k|k}={\Sigma }_{k|k}^{\left(N\right)}$

Outputs: ${\widehat{R}}_{k|k}$,${\Sigma }_{k|k}$

The number of N iterations is also a crucial parameter for proposed approach and, generally, it should be set to a value larger than the system dimension to guarantee the local convergence of variational Bayesian iterations; nevertheless, a too larger value is sure to increase the computational cost for algorithm implementation. The balance between precision and cost should be considered according to the detailed applications.

4. Numerical Simulations

To further demonstrate the performance of the variational Bayesian iteration-based invariant Kaman filter, the attitude estimation system (6) (7) is simulated with parameter ${\Sigma }_{0|0}={0.5236}^2{I}_{3\times 3},\Sigma { }_w={0.01745}^2{I}_{3\times 3},b^{’}={[1,0,0]}^T,b^{’}{}^{’}={[0,1,0]}^T,{\Sigma }_{v’}={0.0873}^2{I}_{3\times 3},{\Sigma }_{v’’}={0.0873}^2{I}_{3\times 3}.$ The real attitude trajectories are generated for 5000 s and the heavy tailed process noise is generated with heavy tailed outliers according to [42], i.e.,

$$p\left( w_k\right)=\left\{\begin{array}{cc}{\mathcal{N}}^c\left( w_k;0_{3\times 1},{\Sigma }_w\right)& \mathrm{w}.\mathrm{p}. 1-\alpha \%\\ {\mathcal{N}}^c\left( w_k;0_{3\times 1},\beta {\Sigma }_w\right)& \mathrm{w}.\mathrm{p}. \alpha \%\end{array}\right.,$$

(64)

which means that $\alpha$ percentage of process noise is drawn from a Gaussian distribution with covariance $\Sigma { }_w$ while the covariance of $\alpha$ percentage of process noise is severely increased by scale parameter $\beta$ due to the presence of outliers.

As a comparison, the filtering methods of the proposed approach (RSIKF), IKF [14,16] the Student’s t filter (STF) [43], the Huber-based Kalman filter (HKF) [44], and the QeIKF [24,25] are conducted together. For HKF, the tuning parameter is set to 1.345 and the number of iterations is selected as 10; in STF the degree of freedom parameter is selected as 3. In the proposed RSIKF, the filter parameters are set to ${\lambda }_k=k+4$, $a_k=2$, $b_k=6$, $N=10$. All above filtering methods are implemented with MATLAB codes on a computer with Intel Core i7-7700 CPU at 3.60 GHz. To compare their performance, the error variable in Lie algebra of 5000 random runs is used to calculate the root mean square error $RMS{E}_k$ during the 5000 runs’ filtering time and average root mean square error $ARMSE$, i.e.,

$$RMS{E}_k\triangleq \sqrt{\frac{1}{5000}{\displaystyle \sum _{l=1}^{5000}{‖{\xi }_{k,l}-{\xi }_{k,l}^{*}‖}^2}}$$

(65)

$$ARMSE\triangleq \sqrt{\frac{1}{5000\times 5000}{\displaystyle \sum _{k=1}^{5000}{\displaystyle \sum _{l=1}^{5000}{‖{\widehat{\xi }}_{k,l}-{\xi }_{k,l}‖}^2}}}$$

(66)

where ${\xi }_{k,l}$ denotes the true estimate at the k-th time instant of the l-th simulation run and ${\widehat{\xi }}_{k,l}$ is the corresponding estimate for the true ${\xi }_{k,l}$;$‖\cdot ‖$ denotes the Euclidean vector norm.

In this work, three cases of heavy tailed process noises are simulated with $\alpha$ = 5, 20 and $\beta$ = 100, 1000 to investigate the filtering performance of different methods. The ARMSE result data of different filtering methods are given in Figure 7 while the $RMS{E}_k$ result data during the first 500 runs’ filtering processes are presented in Figure 8 and Figure 9.

The ARMSE result of IKF in Figure 7 and $RMS{E}_k$ data of IKF in Figure 8, Figure 9, Figure 10 and Figure 11 are always the largest in all methods for all cases, which certifies that the estimation performance of IKF is severely corrupted by heavy tailed outliers.

In STF, the noise distribution is assumed as a given Student’s t distribution, and the ARMSE result in Figure 7 and $RMS{E}_k$ in Figure 8, Figure 9, Figure 10 and Figure 11 is smaller than that of IKF. However, since it still requires accurate prior knowledge of the noise distribution parameter, the unknown or inaccurate parameters still mislead the estimate results.

The QeIKF aims to estimate, online, the unknown noise covariance parameter and also output better results than IKF and STF; however, it assumes the distribution as an Gaussian one so its ARMSE result in Figure 7 and $RMS{E}_k$ in Figure 8 for the case $\alpha$ = 5 become worse than the case of $\alpha$ = 20 in Figure 9; while, as shown in Figure 10 and Figure 11, its filtering precision would be significantly degraded by large outliers with $\beta$ = 1000.

HKF shows some robustness to heavy tailed outliers IKF, STK, and QeKF for the cases of $\beta$ = 1000 as in Figure 10 and Figure 11. Note that, the estimate given by HKF is rather conservative and its robustness is at the cost of inferior precision.

In all simulation cases of $\alpha$ and $\beta$, the ARMSE and $RMS{E}_k$ result of proposed RSIKF is always the least one in Figure 7, Figure 8, Figure 9, Figure 10 and Figure 11, demonstrating its superiority filtering performance to the other available methods. For RSIKF, different setting of N from 1~15 are conducted with all cases of $\alpha$ with the ARMSE result given in Figure 12; obviously, N = 10 already guarantees the iterative convergence and a larger N would significantly increase the computational cost.

5. Conclusions

For aerospace, satellite, and robotics engineering, the matrix Lie groups attitude estimation problem with heavy-tailed process noise is investigated. An improved robust Student’s t invariant Kalman filtering is proposed based on a hierarchical Gaussian state-space model. The probability density function of state prediction is defined based on student’s t distribution, while the conjugate prior distributions of the scale matrix and degrees of freedom (dofs) parameter are respectively formulated as the inverse Wishart and Gamma distribution. For attitude estimation state-space model, the Lie groups rotation matrix of spacecraft attitude is inferred together with the scale matrix and dof parameter using the variational Bayesian iteration. The new approach can improve the filtering robustness of the invariant Kalman filter for Lie groups spacecraft attitude estimation problems with heavy-tailed process uncertainty. Some further applications of the proposed methods can also be extended to other problems such as state smoother, parameter identification, state observers, and so on.