Variational Bayesian Iteration-Based Invariant Kalman Filter for Attitude Estimation on Matrix Lie Groups

: Motivated by the rapid progress of aerospace and robotics engineering, the navigation and control systems on matrix Lie groups have been actively studied in recent years. For rigid targets, the attitude estimation problem is a benchmark one with its states defined as rotation matrices on Lie groups. Based on the invariance properties of symmetry groups, the invariant Kalman filter (IKF) has been developed by researchers for matrix Lie group systems; however, the limitation of the IKF is that its estimation performance is prone to be degraded if the given knowledge of the noise statistics is not accurate. For the symmetry Lie group attitude estimation problem, this paper proposes a new variational Bayesian iteration-based adaptive invariant Kalman filter (VBIKF). In the proposed VBIKF, the a priori error covariance is not propagated by the conventional steps but directly calibrated in an iterative manner based on the posterior sequences. The main advantage of the VBIKF is that the statistics parameter of the system process noise is no longer required and so the IKF’s hard dependency on accurate process noise statistics can be reduced significantly. The mathematical foundation for the new VBIKF is presented and its superior performance in adaptability and simplicity is further demonstrated by numerical simulations.


Introduction
Attitude determination is a benchmark and important problem in astronautic engineering for the state estimation and control of spacecraft and robotic systems [1][2][3][4]. As a widely used attitude estimator, the Kalman filter (KF) is an optimal, numerically efficient, and widely used tool that infers based on all available information, i.e., the dynamic model of the system, sensor data, and the probabilities of both sensor signals and the algorithm's numerical behavior [5,6]. Generally, the state estimation system is usually modeled as vector state space model in Euclidean space and solved with a Kalman filter for vector state variables. In recent years, building navigation and control systems on matrix Lie groups have been actively studied and the properties of the group system provide practical convenience for the definition and simplification of navigation and control system models [7,8]. For engineering applications, the Euclidean system models are usually corrupted by unknown noises or unpredictable disturbances; this is also true for the attitude estimation problems defined on matrix Lie groups and the performance of employed filtering methods is prone to be influenced by inaccurate covariance parameters [6,9,10]. Therefore, for attitude estimation systems on matrix Lie groups, this work aims to study the Kalman filtering problem with unknown noise statistics parameters.
For attitude representation, the widely used quaternions can present a uniform approximation of attitude representation without gimbal-locks [1,9]. The quaternion-based algorithms do not need to compute trigonometric functions and allow for the writing of attitude changes as matrix-vector products. The multiplicative extended Kalman filter is an ad hoc modification of the extended Kalman Filter that accounts for the constraint of a unit quaternion, but its convergence property is ensured only at equilibrium points and the antipodal quaternions should be identified [11]. Recently, there has been significant research interest in the estimation and control of matrix Lie group systems, including state observers and filters for attitude estimation on SO(3) and SE(3) [12,13]. Building attitude kinematics on matrix Lie groups can usually preserve the geometrical property of group systems and the resulting attitude representation does not suffer from the problems of singularities and unwinding [11]. Nevertheless, these studies only generalize the extension to Euclidean Kalman filter theory for attitude estimation on matrix Lie groups and the symmetry properties of attitude estimation models are not sufficiently exploited.
Accounting for the invariance property of Lie groups' attitude kinematics will lead to well-posed problems and boost the performance of attitude estimation algorithms [14]. According to the theory of the invariant Kalman filter (IKF), using the mapping between matrix Lie groups and Lie algebra, attitude estimation systems on matrix Lie groups can be equivalently mapped into a Euclidean vector space and, therefore, the classical Kalman-type estimation methods can be applied to solve the corresponding problems [15,16]. The implementation steps of the IKF are derived based on the invariance and log-linearity of the linear invariant error, which contributes to better filtering convergence on a much bigger set of state trajectories [16]. The necessary probability theory for uncertainty definition on matrix Lie groups has been studied in [17,18] and the concept of a concentrated Gaussian distribution has been employed to describe the noisy and random variables on matrix Lie groups [19,20]. Note that the IKF actually obeys the classical Kalman theory and so its estimation performance also heavily depends on the parameters of the noise covariance matrices being accurate; however, in aerospace and satellite engineering, accurate noise statistics parameters are usually not available due to the presence of unpredictable noises and disturbances, which are sure to have a negative influence on the estimation performance of the invariant Kalman filter [21,22].
Motivated by above discussion, it is meaningful to further improve the filtering performance of the invariant Kalman filter for the matrix Lie group attitude estimation problem in the presence of unpredictable noises and disturbances. Note that, for conventional Euclidean space filtering problems, some adaptive techniques have already been developed [23,24] and the most common way is to directly scale or estimate the unknown noise parameters, i.e., the process and observation noise covariance [25]. Recently, variational Bayesian (VB) methods have been applied to the joint estimation of system state and unknown noise parameters [26,27]. A novel adaptive Kalman filter utilizes the VB technique to obtain an approximate inference for inaccurate process and measurement noise covariance [28]. Note that, to the best of our knowledge, for matrix Lie group systems there remains no investigation on adaptive methods for invariant Kalman filtering.
Therefore, for aerospace, satellite, and robotics engineering, this work aims to investigate the benchmark matrix Lie group attitude estimation problem. Note that, in practice, the noise parameter of onboard observation sensors usually depends on the utilized sensing technique and can be determined offline, but it is impractical to accurately evaluate the unpredictable disturbances in the kinematics/dynamics model [23,24]. In this work, the matrix Lie group attitude estimation problem with inaccurate process noise covariance is investigated and a variational Bayesian iteration-based adaptive invariant Kalman filter (VBIKF) is proposed. In the VBIKF, the a priori error covariance is not propagated by the conventional steps but directly calibrated in an iterative manner based on the posterior sequences. The main advantage is that the statistics parameter of the system process noise is no longer required and so the IKF's hard dependency on accurate process noise statistics can be reduced significantly. The mathe-matical foundation for the new VBIKF is presented and its superior performance in adaptability and simplicity is further demonstrated by numerical simulations.

Primaries and Problem Definition
This section first presents the essential primaries about matrix Lie groups and the uncertainty definition, which constitute the fundamental theory of the matrix Lie group attitude estimation problem. The attitude estimation problem is described with discussions on the symmetry invariance property of the attitude system. Then, the invariant Kalman filter is introduced and its heavy constraint on the noise parameter is also presented in the problem definition of this work.

Matrix Lie Groups and the Concentrated Gaussian Distribution
In this paper, a matrix Lie group is characterized as the set of square, invertible m m  matrices satisfying the matrix operations of multiplication and inversion without going outside [29][30][31].
where the m m  identity matrix d I is the group identity element.
For every matrix Lie group G, there is an associated Lie algebra .
As a linear space, the Lie algebra g of p-dimensional matrix Lie group G is related to the p-dimensional Euclidean vector space p  through the natural linear isomorphism     : and : The uncertainty representation of additive noise in Euclidean vector space cannot be directly applied to matrix Lie group space. The Baker-Campbell-Haussdorff formula [19] provides an approximate to the product of Lie groups in p  [29,30], i.e., for , is the negligible second-and higher-order terms if a and b are small enough. In a concentrated Gaussian distribution (CGD) [19,20,32], a random Lie group variable χ G  has a CGD with mean G   and covariance is a CGD random variable with mean d I and covariance  ; and  is a zero-mean normally distributed random variable with covariance  .

The Attitude Estimation Systems on Special Orthogonal Group SO(3)
One motivating example of estimation on matrix Lie groups is the attitude determination on special orthogonal group 3 3 ( for a rigid spacecraft with model [1,14]. where denotes the rotation matrix from the body frame to the as the corresponding random process . The associated Lie algebra g is actually the space of 3 × 3 real skew-symmetric matrices, i.e., the mapping from the vector to the corresponding skew-symmetric matrix has the form of (8) and its relations to group element R are (9) and (10) [31].
where   denotes the common skew-symmetric operation for vector 3    ;  represents the standard Euclidean vector norm; and the  in (10) , then the motion of k  can be obtained as.
where the last step is a first-order approximation of the BCH formula (4). In the evolution model (11) for k  , the true state 1 k R  and the control input have been cancelled out, which means that the motion of k  is independent of the true state trajectory and control input

The Invariant Kalman Filter for Attitude Estimation
In the invariant Kalman filter (IKF), if given the initial , the state prediction using the deterministic part of the dynamics (6) is: Resorting to the equivalence of k R and k  in the probability distribution, the prior error covariance As to the steps for updating the invariant Kalman filter, the two-vector-based observation 6 k Y   can be reformulated to define the new innovation vector are respectively the new observation matrix and the noise with Then, the steps for the correction of the IKF are: where k K is the gain matrix correction term, and the vector term k k K z is projected into the matrix Lie group space through the matrix exponential operation in (15) [16,17].

The Constraint on the Invariant Kalman Filter for Attitude Estimation
In IKF theory, the matrix Lie group system (6) and (7) for rotation k R is converted to the Euclidean vector space system (11) and (14) for error k  ; therefore, according to the equivalence of k R and k  in the probability distribution, the propagation of the covariance and gain parameters in the IKF can mimic that of the classical Kalman filter [14][15][16]. However, while the IKF resembles the simple and elegant filtering steps of the classical Kalman filter, at the same time it also inherits the hard constraint that the system model and noise statistics parameters should be accurately given; if the noise covariance parameters are not correct, the estimate results are rather prone to being biased. Nevertheless, for some aerospace and astronautics applications, the noise parameter of the observation sensors usually depends on the utilized sensor technique and could be tuned by offline tests; however, it is too difficult to precisely determine the statistics of unpredictable disturbances that are often influenced by the operating environment of different target missions [1,21,23,24].
Note that, for conventional Euclidean space filtering problems, some adaptive techniques have already been developed but, to the best of our knowledge, for matrix Lie group systems there remains no investigation on adaptive methods for invariant Kalman filtering. Therefore, this work focuses on adaptive invariant Kalman filtering methods for matrix Lie group attitude estimation problems without an accurate process noise covariance w  and attempts to remove the IKF's hard constraint on an a priori and accurate w  .

Variational Iteration-Based Invariant Kalman Filter for Attitude Estimation
In the invariant Kalman filter, given the prior error covariance matrix , the predicted probability density function (PDF)   where the prior estimate | 1 k k R  is obtained with (12) and the prior error covariance is propagated through the calculus (13) with w  required. Obviously, if the true value of the process noise covariance w  cannot be accurately determined in advance, the is sure to be misled by an incorrect w  , which will in turn degrade the estimation results of the IKF.

Distribution Definition for the Prior Error Covariance
To deal with the trouble caused by an inaccurate w  , in this work we choose to infer the rotation group state | 1 k k R  together with the prior error covariance . The basic idea is that, before calculating the required PDF   Therefore, to infer the prior error covariance , the following inverse Winshart distribution is employed in this work to describe the distribution of are respectively the degree of freedom parameter and the inverse scale matrix parameter for the PDF    that needs to be determined.

Variational Bayesian Approximations of Posterior PDF
To infer the rotation group state The optimal solution to (23) can be obtained based on the following equations: Note that the PDF   k k p Y R is actually the distribution of the noisy observation k Y conditioned on the true rotation state k R and, according to (7) and (14), we have: is actually the prior distribution of the rotation group state k R ; then, according to the equivalence of k R and k  in the probability distribution, we have: Then, using (24) and (25) in (28), we have: where     1 i q   is the iterative approximation of PDF   q  at the i+1th iteration;     1 E i   represents the mathematical expectation at the ith iteration; and   i k  is given by: is the correction term at the i+1th iteration and will be calculated later. Note that, since the PDF of is considered to be an inverse Wishart distribution, the updated PDF Then, according to (20), the (30) can be calculated as:

The Variational Bayesian Iteration-Based Invariant Kalman Filter
Define the propagated     at the i+1th iteration as: then the propagated PDF at the i+1th iteration can be written as: Then, using (27) and (36)~(38) in (30) yields: According to the above equations, the     times that of the conventional IKF. Obviously, an N that is too large is sure to increase the computational cost of the algorithm's implementation and so a balance between precision and cost should be considered according to the particular application.
As to the ARMSE result displayed in Figure 1 and Table 1, if the scale parameter  is close or equal to 1 (  = 1), the employed covariance ˆw  is close to its true w  . Then, the ARMSE value of the standard IKF is the lowest among the three methods and its performance is optimal in the least square sense, which is also certified by the data on k RMSE displayed in Figure 2. However, when  becomes larger (for instance,   2), the accuracy of the employed ˆw  is biased by the scaling parameter  and the ARM-SE value of the IKF increases significantly, which means that the optimal estimation performance of the IKF is destroyed by the inaccurate ˆw  . We can conclude that the performance of the IKF is rather sensitive to the accuracy of ˆw  and, for cases of an inaccurate ˆw  , adaptive methods are necessary to improve the performance of attitude estimation.   Figures 4-7; however, the estimation precision is still degraded by the inaccurate parameter and its effective range is limited. For the cases of 6   , the estimation precision is still degraded by the inaccurate parameter and its effective range is rather small and limited. Moreover, as the k RMSE data show in Figures 5-7, the process data on the QeIKF show some instability during the initializing stage; in fact, the critical issue with the QeIKF is that its fil- tering is not stable and rather prone to divergence [28]. In conclusion, the filtering precision and stability of the available QeIKF could not effectively reduce the influence of the inaccurate ˆw  .

Conclusions
For aerospace, satellite, and robotics engineering, the matrix Lie group attitude estimation problem with an inaccurate process noise covariance was investigated and a variational Bayesian iteration-based adaptive invariant Kalman filter (VBIKF) was proposed. In the VBIKF, the a priori error covariance is not propagated by the conventional steps but directly calibrated in an iterative manner based on the posterior sequences. The main advantage is that the statistics parameter of the system process noise is no longer required such that the IKF's hard dependency on accurate process noise statistics can be reduced significantly. The numerical simulation results presented demonstrate the superior performance of the proposed VBIKF in terms of filtering adaptability and simplicity.
Author Contributions: Data curation, conceptualization, methodology, writing-review and editing, software and funding, J.W.; writing-review and editing, software and validation, Z.C. All authors have read and agreed to the published version of the manuscript.

Data Availability Statement:
The data presented in this study are available on request from the corresponding author. The data are not publicly available as the data also forms part of an ongoing study.