Adaptive invariant Kalman ﬁltering for attitude estimation on SO (3) thorough feedback calibration of prior error covariance

For invariant attitude dynamics evolving on matrix Lie groups, by proposing the stochastic feedback–based covariance calibration scheme, an adaptive invariant Kalman ﬁlter (AIKF) is elaborated to deal with the attitude estimation problems corrupted by unknown or inaccurate process noise statistics. The invariant Kalman ﬁlter (IKF) takes into account the geometry property of attitude dynamics and can boost the estimation performance; how-ever, IKF requires accurate knowledge of the noise statistics and an incorrect noise parameter is prone to deteriorating the precision of ﬁnal estimates. To eliminate this impact, instead of using the original covariance propagation step of IKF, the prior error covariance of the proposed AIKF is online calibrated based on the posterior information of the feedback stochastic sequence. As the main advantage, the statistics parameter of system process noise is no longer required in the proposed AIKF and the negative inﬂuence by unknown/incorrect noise parameters can be reduced signiﬁcantly. The mathematical foundation for the new adaption scheme of AIKF is also presented. The AIKF’s advantage in ﬁltering adaptability and simplicity


INTRODUCTION
Motivated by the navigation and control of aerospace engineering and robotics applications, the dynamics and kinematics of Lie groups have been actively studied for the past decade [1][2][3][4][5][6].
The attitude estimation problem whose states evolve into Lie groups with invariance properties is a benchmark and motivating application, and various Kalman type filtering methods have been developed by researchers for attitude estimation [7,8], target tracking and so on [9,10]. Note that, in robotics or satellite engineering the attitude kinematic models might be corrupted by noisy disturbances and the accurate noise statistics is usually unavailable [9,10], which is prone to degrade the performance of Kalman type attitude estimators [11,12]. So, for attitude models on matrix Lie groups, this work focuses on the Kalman filtering problems with no accurate noise statistics.
Researchers and practitioners have typically used unconstrained three-set parameterizations of the rotation matrix or unit quaternion. The multiplicative extended Kalman filter [13] corresponds to an ad hoc modification of usual EKF and takes into account the constraint of a unit quaternion. Nevertheless, it only recovers the convergence property at equilibrium points and has to identify antipodal quaternions with a single attitude [14]. In recent years there has been significant interest in performing estimation and control directly on matrix Lie groups SO(3) [15]. Some state observers and filters for attitude estimation on SO (3) and SE (3) have been reported in [16][17][18][19] and they do not suffer from kinematic singularities and unwinding [14]. Note that, these researches try to devise general adaptations of Kalman theory to matrix Lie groups but they still do not exploit the geometrical properties of the specific invariant case [20,21].
Taking into account the geometry property of a Lie group system usually leads to well-posed problems and can boost the performance of estimation algorithms [17]. For attitude estimation of Lie groups, the invariant extended Kalman filter (IEKF) was originally introduced in [22] and continued in [13,16,23,24]. In invariant Kalman filter (IKF), the prediction and correction steps are derived based on the linear invariant error and then the gain and covariance equations will converge to constant values on a much bigger set of trajectories, which means a better convergence property [20]. The use of Lie groups for state estimation has recently spanned a range of applications and a rich stream of theoretical results [25,26] have demonstrated the importance of using probability distributions on Lie groups notably for attitude estimation [27][28][29][30].
According to invariant Kalman filtering theory, Lie group attitude estimation problems can be projected into Euclidean space and resolved by mimicking classical Kalman filtering steps [31]. However, Kalman theory's prior constraint on the knowledge of noise statistics is inevitably inherited by IKF and its filtering performance is rather sensitive to the accuracy of noise statistics. For aerospace and robotic applications, the Lie groups kinematic models of a robot or a satelite are usually corrupted by noises and disturbances [8,13,16]; the noise statistics parameter of onboard sensors might be offline tuned by experimental tests, but in practice it is too difficult to isolate process noise from attitude model and determine the covariance parameter in advance [32]. In Euclidean space, various adaptive approaches have been studied to deal with the trouble of inaccurate noise parameters [33], but until now there is no special study on developing an adaptive version of IKF in the literature and it is still necessary to investigate a new adaptive approach to the attitude estimation problem for matrix Lie groups.
Motivated by the above discussion, by proposing the stochastic feedback based covariance adaption scheme for IKF, a new adaptive invariant Kalman filter is elaborated for attitude estimation problems without accurate knowledge of process noise covariance. The main innovation of this work is that, instead of using the conventional IKF step, in new adaptive IKF (AIKF) the prior error covariance is online calibrated using a posterior stochastic sequence, which significantly improves the IKF's filtering adaptability and accuracy and is also demonstrated by the presented numerical simulation results.

Matrix Lie groups and the concentrated Gaussian distribution
A matrix Lie group G is a set of square invertible matrices of size m × m on which matrix multiplication and inversion can be safely used without going outside the set, i.e. I m×m ∈ G , ∀ ∈ G , −1 ∈ G , ∀ 1 , 2 ∈ G , 1 2 ∈ G , where I m×m is the identity matrix. The dimension of G and the associated Lie algebra ⊂ ℝ m×m is denoted by d = dim (m ≤ d < m 2 ). Then, where exp(⋅) and log(⋅) are the matrix exponential and logarithmic operation respectively; Exp G is the Lie exponential operation with the inverse Exp Since the usual uncertainty representation of additive noise in ℝ d cannot be directly applied in Lie group space G , the multiplication form of concentrated Gaussian distribution has been defined [31,34] to describe the probability distribution on Lie groups, i.e. for the random variable ∈ G , where ℕ L and ℕ R denote the distributions on matrix Lie groups based on left and right multiplication, respectively; ℕ is the Gaussian distribution in ℝ d ;̄∈ G is the expectation of in Lie group space G; ∈ ℝ d is a zero-mean Gaussian noise vector; in the definition of concentrated Gaussian distribution, noise and all the eigenvalues of covariance P ∈ ℝ d ×d are assumed to be small enough [31], thus, almost all the mass of the distribution is concentrated in a small neighbourhood around̄. Owning to the noncommutativity of matrix multiplication, the left multiplication version ℕ L is different from ℕ R and, in this work, the right multiplication probability definition ℕ R is employed in the attitude estimation problem.

Attitude estimation problem on Lie group SO(3)
Consider the discrete attitude dynamics on SO(3) associated with observations as follows [17,30]: where R k ∈ G = SO(3) represents the rotation matrix that maps the body frame to the earth-fixed frame at time instant k, Ω k ∈ G is the control input encoding the instantaneous rotation according to attitude evolution model, the Gaussian white process noise w k ∼ ℕ(0 d ×1 , Q w ) ∈ ℝ 3 denotes the small process noise injected through the Lie exponential term Exp G (w k ), Y k ∈ ℝ 6 is a composition of the discrete noisy observations y ′ k ∈ ℝ 3 and y ′′ k ∈ ℝ 3 with known vector b ′ ∈ ℝ 3 and b ′′ ∈ ℝ 3 (for instance the gravity and the magnetic field); v ′ k , v ′′ k ∈ ℝ 3 are the independent isotropic white noises and their covariance are diagonal matrices and respectively represented by Q v ′ and Q v ′′ .
denote the invariant error projected from the multiplicated form of distance between the true state R k and estimateR k (sim- , then the evolving motion of the projected error k in ℝ 3 is independent of the true value of Lie groups model parameter Ω k−1 , Proof. According to the above definitionR is the Baker-Campbell-Hausdorff (BCH) formula. Note that, in previous context about the concept of concentrated Gaussian distribution, the projected error k = Exp −1 G (R k R −1 k ) and w k−1 have been assumed to be small so that the higher-order term can be neglected as in (3).
Remark 1. This theorem describes the state-trajectory independent property of the invariant error k : the true state R k−1 and the model parameter Ω k−1 in (1) have been cancelled out in the derivation process and, although k = Exp −1 G (R k R −1 k ) is defined using the estimateR k and true state R k−1 , the evolution of the projected invariant error k is independent of the true trajectory of state R k−1 as well as the parameter Ω k−1 . More theoretical details can be found in [20,30].
As the consequence of the above theorem, the probability distribution of the Lie group state R k based on observations is equivalent to that of error k in ℝ 3 , which actually constitutes the theoretical basis of the invariant Kalman filter.

Invariant Kalman filtering theory
For attitude estimation, the invariant Kalman filter (IKF) is to recursively predict and correct the estimate for the Lie group state and its covariance. Given the initialR 0|0 ∼ ℕ R (R 0 , P 0|0 ), the state prediction using the deterministic part of dynamics (1) isR whereR k|k−1 andR k−1|k−1 denote the prior and posterior error estimate for true state. Resorting to the equivalence of R k and k in probability distribution, the prior error covariance P k|k−1 forR k|k−1 is propagated according to the evolution of k in (4) where P k−1|k−1 denotes the covariance of the posterior estimatê for k and also the covariance of R k|k −1 In the correction step of IKF, the innovation vector z k ∈ ℝ 6 is redefined and linearized as [25,30] where H = since v ′ k and v ′′ k are independent isotropic andR k|k−1R T k|k−1 = I 3×3 . Then the posterior estimateR k|k and covariance P k|k can be obtained asR where Δ̂k ∈ ℝ 3 is the correction term. Note that, to update the state estimateR k|k of the Lie group G , the vector space term Δ k is projected into the Lie group space with a Lie exponential operation as in (8).
Remark 2. According to Theorem 1, in the above IKF, the original system (1) and (2) for Lie group state R k is converted to the vector space system (3) and (7) for error k and, thus, the calculation of covariance P k|k−1 , P k|k and Kalman gain K k for IKF can mimic that of the classical Kalman filter. More detailed results about IKF can be found in [25,26,30].
Problem. Note that IKF mimics the Kalman filter for vector space system (3) and (6) but, inevitably, it also inherits the critical requirement on the a prior and accurate knowledge of noise statistics, i.e. the Q w in (5) and Q v in (9). For some aerospace and robotic applications, the parameter Q v of onboard observation sensors can usually be determined through offline tests, but it is impractical to obtain the accurate Q w [32,33], which makes some trouble for the practical implementation of IKF and even deteriorates the final estimate results.
So this work focuses on the invariant Kalman filtering problems with an inaccurate/unknown process noise covariance and tries to relax IKF's constraint on the a priori and accurate Q w without sacrificing IKF's simplicity and elegance.

New adaptive invariant Kalman filter
According to the above steps of IKF, an incorrect covariance Q w would mislead the propagation of prior error covariance P k|k−1 which is a critical parameter for the calculation of the Kalman gain matrix K k . To eliminate the impact of an incorrect Q w , the conventional step (5) requiring Q w should be removed and, to calculate the prior error covariance P k|k−1 , this work proposes the feedback calibration based adaption scheme: whereP k|k−1 denotes the estimate for true prior error covariance P k|k−1 and ΔP k denotes the feedback correction term. Note that, the Kalman gain K k−1 is still calculated as (10) but the original P k|k−1 should be replaced by theP k|k−1 estimated using (11); the posterior error covariance P k|k is not used in AIKF and related calculations can be dropped off. The main steps of the proposed adaptive invariant Kalman filter (AIKF) are concluded in the iteration part of Algorithm 1 and the diagram is given in Figure 1.

ALGORITHM 1 The detailed steps of proposed AIKF
Preparation (k < k 0 ) InitializeR 0|0 and P 0|0 ; Use the steps (4)-(10) of IKF with the available inaccurate Q w to help achieve a rough steady filtering before k 0 ; Initialize the estimated prior error covarianceP k 0 |k 0 −1 = P k 0 |k 0 −1 ;

FIGURE 1 The filtering diagram of the proposed AIKF
The key innovation of the proposed AIFK lies in that the prior error covariance is directly modelled using a posteriori sequence. Obviously, Q w no longer appears in the filtering diagram of AIKF as in Figure 1, therefore IKF's critical constraint on Q w is relaxed by the proposed scheme (11) and (12) for AIKF.
Remark 3. Since proposed scheme (11) and (12) depends on the filter parameters of the last instant such asP k−1|k−2 and Δ̂k −1 , for practical implementation, it is better to use IKF with the available inaccurate Q w to help achieve a roughly stable state for the initialization ofP k−1|k−2 , Δ̂k −1 as the preparation in Algorithm 1. Note that, without the accurate Q w the optimal filtering state is almost impossible, but the rough steady and stable filtering can be achieved after some time instants (denoted as k 0 ) by tuning the initial guess for true Q w . The parameter k 0 depends on the detailed problem at hand and is usually set to a value between two and three times of the state dimension (e.g. k 0 = 8 for attitude estimation in Section 4).
Remark 4. For the iteration steps of Algorithm 1, the termination time is determined by online comparing the eigenvalues of the correction term ΔP k andP k|k−1 . If the eigenvalues of ΔP k are far less than the corresponding eigenvalues ofP k|k−1 , then ΔP k is neglegiable respect toP k|k−1 and the iteration process could be considered to have already arrived at the steady state.

3.2
Theoretical derivation of the covariance adaption scheme Lemma 1 ([35]). For the obviously controllable and observable linear time-invariant model (3) and (6), the covariance P k|k−1 of steady optimal Kalman filtering will converge component-wise to the constant solution matrix of the matrix Ricatti equation.
Remark 5. The optimal steady constant P k|k−1 and the resulted Kalman gain matrix K k could be offline obtained a prior and then stored for online use, which has been named as the limiting Kalman filter in [35] or constant gain Kalman filter in [36]. (1) and (2), if invariant Kalman filter is proceeding at its optimal steady state, according to Lemma 2 and 3, following assumptions are valid:

Theorem 1. For matrix Lie groups attitude model
1. the P j | j −1 of all time instants could be regarded as a constant in optimal filters, i.e. P k|k−1 = P j | j −1 , j < k; 2. the innovation sequence z j is a normally distributed noise with its covariance being S k = H P k|k−1 H T + Q v . and then, a maximum likelihood estimate for the optimal P k|k−1 of the invariant Kalman filtering can be obtained by (11) and (12).
Proof. According to Theorem 1, the matrix Lie groups system (1) and (2) can be equivalently converted to Euclidean space model (3) and (6) as described in Section II, so the filtering parameters of invariant Kalman filter for model (1) and (2) are the same as classical Kalman filter for (3) and (6). Then, with Lemma 1 and 2, the assumptions about P j | j −1 being constant and z j being Gaussian white are valid for optimal filtering with (1) and (2).
Let e k = {z 1 , z 2 , … , z k−1 } be the set of innovation sequences before instant k andP k|k−1 denote an estimate for the unknown P k|k−1 based on the set e k , according to the above two lemmas the maximum likelihood function can be formulated as follows: where p(e k |P k|k−1 ), p(z j |P k|k−1 ) are the respective probability density of e k and z j conditioned on the estimateP k|k−1 , where det(⋅) is the determinant operator. Solve L(P k|k−1 )∕P k|k−1 = 0 d ×d with the mathematical operations as in [38], Pre-and post-multiply (15) with the approximated constant P j | j −1 = P k|k−1 , Using (9) and S k = H P k|k−1 H T + Q v , And then Then with the assumption P k|k−1 = P j | j −1 , the estimatedP k|k−1 could be approximated by averaging the P j | j −1 of historical instants,
Although the two assumptions of constant P j | j −1 and Gaussian white noise z j are strictly valid for the optimal steady filtering state, in [39,40] they have already been used to seek an explicit and simple approximation for unknown parameters.
To improve the quality of these approximations, the IKF is employed before starting the new approach as in Algorithm 1 to help the filter achieve a rough convergence, and the validity of this approach is demonstrated by the following simulations.

NUMERICAL SIMULATIONS
To investigate the performance of the proposed AIKF for attitude estimation, the attitude model (1) and (2) is simulated using The covariance error index k during the filtering process of IKF, QeIKF and AIKF for differentQ w the parameter P 0|0 = 0.
The total steps are set to 10,000 and the results of 1000 random simulation runs are evaluated. Note that, true attitude trajectories are generated with the real value of Q w , but in practical applications there is no access to the accurate Q w . In order to study the filtering adaptability to Q w of various quality, the filtering tests of AIKF, the QeIKF [38,40] and IKF using the roughQ w = Q w × diag([ , 1∕ , 1]) with ranging from 0.1 to 10 are implemented. To implement the AIKF and QeIKF, the filtering instants before k 0 is initialized using the IKF with the inaccurateQ w and, for this attitude model, at the time step k 0 = 8 the covariance parameters of IKF gradually converges. The error variable is expressed in Lie algebra and the root mean square error RMS E k at time instant k and the average root mean square error ARMSE is calculated as the performance metric, i.e.

RMS E k
where the subscript l marks the corresponding Monte Carlo simulation run and k denotes the particular time instant,̂1 k,l is the first element of the estimated Lie algebra state vector, i * k,l refers to the ith component of reference true state.
To study the performance of the proposed adaption scheme (11), the covariance error index k are also calculated as where ‖D‖ = tr(DD T ),P l k|k−1 is the prior error covariance P k|k−1 in IKF and QeIKF or the estimatedP k|k−1 in AIKF at the lth simulation, andP l o,k|k−1 is optimal P k|k−1 of IKF with the real value of Q w . It is obvious that, with a smaller covariance error index k , the propagatedP l k|k−1 is closer to the optimal one and the estimation accuracy of the corresponding filtering approach is sure to be better. Generally, the covariance error index of an optimal IKF with accurate Q w will gradually converge to zero.
As the simulation result given in Figure 2, when the parameter is around 1 the availableQ w is close to the true value of Q w , so the ARMSE of both IKF and AIKF are better than QeIKF and near the optimal; but, when becomes larger or smaller, the accuracy ofQ w is gradually destroyed and the ARMSE result of IKF increases significantly, certifying IKF's critical dependency on accurate Q w . Although the ARMSE data of QeIKF is smaller than IKF for < 0.5 and > 2, but it is still prone to be influenced by the quality of the initial guessQ w . As to the new AIKF, although its ARMSE data is still related to , it is obvious that the ARMSE result of AIKF is rather less sensitive than other filters, which certifies AIKF's advantage in filtering adaptability to inaccurateQ w .
For the cases of = 10, 6, 0.4 and 0.1, the process noise covarianceQ w of different accuracy are constructed; the RMS E k results during whole filtering processes of IKF, QeIKF and AIKF are displayed in Figure 3. Obviously, the smaller RMS E k data of AIKF shows that its filtering precision is better than QeIKF and IKF for all cases, which certifies AIKF's superior filtering adaptability to inaccurateQ w .
The covariance error index k during the filtering process for = 10, 6, 0.4 and 0.1 are also presented in Figure 4. Note that, if given the accurate Q w the prior error covariance of IKF will converge to the optimal value and its covariance error index k will then become zero; but, with the Q w of different accuracy as = 10, 6, 0.4 and 0.1, the covariance error index k of IKF converge to some nonzero value as in Figure 4. The covariance error index k of QeIKF for = 10, 6, 0.4 and 0.1 gradually decreases as the filtering processes proceed, but in the four cases in Figure 4 the covariance error index k of the new approach AIKF is always the least, which again demonstrates that the prior error covariance estimated by the proposed scheme (11) significantly improves the filtering adaptability of AIKF to the Q w of different accuracy.

CONCLUSION
For Lie group attitude estimation problems with no accurate process noise statistics, this paper proposes an adaptive version of the invariant Kalman filtering method. To reduce the negative impact of inaccurate covariance parameters, a new covariance propagation scheme was elaborated based on feedback evaluation of the posterior sequence, which significantly improves the adaptability of the invariant Kalman filter and constitutes the main contribution of this work. The mathematical derivation of the proposed approach is also presented and the numerical simulations further certify AIKF's superior adaptability and accuracy.