Improved Cubature Kalman Filtering on Matrix Lie Groups Based on Intrinsic Numerical Integration Error Calibration with Application to Attitude Estimation

Abstract

This paper investigates the numerical integration error calibration problem in Lie group sigma point filters to obtain more accurate estimation results. On the basis of the theoretical framework of the Bayes–Sard quadrature transformation, we first established a Bayesian estimator on matrix Lie groups for system measurements in Euclidean spaces or Lie groups. The estimator was then employed to develop a generalized Bayes–Sard cubature Kalman filter on matrix Lie groups that considers additional uncertainties brought by integration errors and contains two variants. We also built on the maximum likelihood principle, and an adaptive version of the proposed filter was derived for better algorithm flexibility and more precise filtering results. The proposed filters were applied to the quaternion attitude estimation problem. Monte Carlo numerical simulations supported that the proposed filters achieved better estimation quality than that of other Lie group filters in the mentioned studies.

1. Introduction

Estimating the state of dynamical systems is a research hotspot and a frequently faced essential issue in many real-life applications, e.g., navigation, target tracking, robotics, and automatic control [1,2,3]. Due to the imperfect system model and the existence of noise, filtering is the most used estimating approach. For linear systems with Gaussian uncertainties, the Kalman filter (KF) [4], as the most desirable estimator established on minimal mean-square-error (MMSE) criterion, can provide an optimal recursive solution. However, while the system is nonlinear, obtaining the optimal closed-form solution is intractable. Researchers proposed various methods via analytical or numerical approximation to address nonlinear state estimation issues. A successful analytical approximation method is the extended Kalman filter (EKF) [5], which employs first-order Taylor series approximation to linearize nonlinear dynamics, but may perform poorly with highly nonlinear systems or large initial errors. Moreover, the main drawback of the EKF is that it needs to calculate the Jacobians, which is relatively tricky for complicated models.

Instead of dealing with nonlinearities by evaluating Jacobians, another class of nonlinear filtering based on numerical approximation is preferred. These filters are collectively called sigma point filters, including the unscented Kalman (UKF) [6], Gauss–Hermite–Kalman (GHKF) [7], and cubature Kalman (CKF) [8] filters. Different deterministic sigma points are used in these filtering algorithms to approximate moments of the probability distribution functions (PDFs). Compared with the EKF, the sigma point Kalman filter can provide better robustness and estimation accuracy, but increases the computational burden. Another problem underlying the sigma point Kalman filter is that errors caused by the traditional numerical quadrature rules may inject bias into the estimated moments without compensation. Concerning that ignoring the integral approximation error may lead to overconfident estimates, several studies employed Bayesian quadrature (BQ) and other quadrature approaches based on BQ theory (such as Gaussian process quadrature (GPQ) and Bayes–Sard quadrature (BSQ)) to improve the sigma point Kalman filter [9,10,11]. These approaches are successful because BQ, GPQ, and BSQ rules can quantify uncertainty in numerical integration.

Most EKFs and sigma point Kalman filters are designed for system dynamical models in Euclidean spaces in the existing literature. Nevertheless, when the system state exists in a Riemannian manifold, considering the geometry of the manifold can have merits, for example, enlarging the convergence domain and increasing the convergence rate [12]. Many works are devoted to introducing Euclidean space filters to manifolds to obtain natural and good metrics. The authors in [13] generalized the unscented transform and the UKF to Riemannian manifolds, and produced a general optimization framework. By extending the main concepts of UKF for Riemannian state-space systems, the authors in [14] proposed a series of more widespread and consistent Riemannian UKFs (RiUKFs) that supplement the theory gap in UKF on manifolds. Another generalized method about UKF on manifolds was presented in [15], which further developed the authors’ previous work of UKF on Lie groups in [16,17].

Implementing KFs on Lie groups is another widely studied line of designing filtering algorithms on manifolds, which was first proposed in [18,19]. Due to the systems’ invariance properties, many studies proposed invariant KFs on Lie groups: the authors in [20,21,22,23] designed intrinsic continuous-discrete invariant EKFs (IEKFs) for continuous-time systems on Lie groups with discrete-time measurements in Euclidean spaces and applied them to attitude estimation and inertial navigation; in [24,25,26], the researchers proposed the invariant UKFs (IUKFs). To help practitioners fully understand IEKFs, Ref. [27] provided three typical examples, and [28] presented the right IEKF algorithm for simultaneous localization and mapping (SLAM). Moreover, the authors in [29,30,31,32] devised EKFs on Lie groups for systems with measurements on matrix Lie groups. Other designs proposed EKF extensions for Lie groups [12,33]. On the other hand, UKF on Lie groups (UKF-LG) was developed in [16,17] for SLAM and sensor fusion. The authors in [34] suggested an impressive UKF based on UKF theory on Lie groups for visual–inertial SLAM. Unlike UKF-LG, in [35], the authors proposed another type of UKF filter for matrix Lie groups. This filter takes the time propagation step on the Lie algebra, improving computation efficiency. In addition, the authors in [36] introduced an invariant CKF on Lie groups for the attitude estimation problem from vector measurements.

Even if the above works succeeded in extending the sigma point filters to the Lie group, these filters still do not consider integration errors caused by quadrature rules. Hence, this paper is concentrated on calibrating the numerical integration error in the sigma point filtering algorithm on matrix Lie groups to improve filtering estimation quality. Motived by the previous study on employing the Bayes–Sard quadrature (BSQ) method in Euclidean sigma point filtering, we attempted to extend the BSQ method to filters on matrix Lie groups. According to the theoretical framework of the Bayes–Sard quadrature moment transformation (BSQMT) on BSQ in [11], we first give a version of BSQMT that selects cubature points as its prepoints, and then leverage the BSQMT to design our method. Lastly, the proposed method was verified by simulations in an attitude estimation system. An attitude estimation system usually consists of gyroscopes that provide angular velocity measurements and at least two vector sensors (such as accelerometers, magnetometers, star trackers, and sun sensors) that provide at least two nonparallel vector measurements. Our main contributions are as follows.

(1)

Giving a generalized system measurement model covering measurements in both Euclidean spaces and Lie groups, and developing a Bayesian estimator by utilizing BSQMT with the generalized measurement model for the state estimation problem on Lie groups.

(2)

Deriving improved cubature Kalman filtering on matrix Lie groups with BSQMT to calibrate numerical integration errors, and introducing a method with the maximum likelihood principle to calculate adaptive expected model variance.

(3)

Applying the proposed Lie group filtering to quaternion attitude estimation problems, and providing numerical simulations to validate the effectiveness of the proposed filtering.

The rest of this paper is constructed as follows. Section 2 reviews the preliminary knowledge of the Lie group. Section 3 outlines a designed Bayesian estimation method on Lie groups through leveraging the BSQMT for two different types of measurements. Section 4 gives detailed derivations of the proposed filtering algorithm on Lie groups. In Section 5, numerical simulations about quaternion attitude estimation demonstrate the effectiveness of the proposed filters. Lastly, in Section 6, we draw conclusions and present further work.

2. Mathematical Preliminary

This section reviews the theory and some basic properties of Lie groups used in this work. The following contents are chiefly based on [22,37] and partly on [15].

2.1. Introduction to Matrix Lie Groups

Matrix Lie group: Let $G\subset {\mathbb{R}}^{N\times N}$ be a matrix Lie group that is a subset of the invertible square matrix and holds properties including group identity, multiplication map, and inversion map: ${\mathit{I}}_{N\times N}\in G$; $\forall {\chi }_1$, ${\chi }_2\in G,{\chi }_1{\chi }_2\in G$; $\forall \chi \in G,{\chi }^{-1}\in G$; where ${\mathit{I}}_{N\times N}$ is the identity matrix of ${\mathbb{R}}^{N\times N}$. The matrix Lie group is also characterized by a smooth manifold structure, so that both multiplication and inversion maps are smooth operations. Each point on G, i.e., $\chi \in G$, can attach to a tangent space at that point, denoted by $T_{\chi }G$. Tangent space $T_{\chi }G$, defined by the derivative of any curve $\gamma \left(0\right)$, where $\gamma \left(0\right)=\chi$ (see the left plot of Figure 1), is a subvector space of ${\mathbb{R}}^{N\times N}$ with equal dimensions to those of G. Among all tangent spaces of G, the tangent space taken at group identity matrix ${\mathit{I}}_{N\times N}$ and denoted as $T_{I}G$ is called Lie algebra.

Lie algebra: Let $\mathfrak{g}$ denote the Lie algebra associated with a d-dimension Lie group; there is $T_{I}G:=\mathfrak{g}\in {\mathbb{R}}^{d\times d}$. As Lie groups and Lie algebras lack mathematical tools, one needs to identify Lie algebra $\mathfrak{g}$ to Euclidean space ${\mathbb{R}}^d$ for convenient calculation. There is a linear bijection between $\mathfrak{g}$ and ${\mathbb{R}}^d$, i.e., ${[·]}_G^{\vee }:\mathfrak{g}\to {\mathbb{R}}^d$. This linear bijection is invertible, and its inverse map is defined by ${[·]}_G^{\wedge }:{\mathbb{R}}^d\to \mathfrak{g}$. For example, assume vector $\eta \in {\mathbb{R}}^d$ and Lie algebra ${\eta }^{\wedge }\in \mathfrak{g}$; then, we have ${\left[\eta \right]}_G^{\wedge }={\eta }^{\wedge }\in \mathfrak{g}$ and ${\left[{\eta }^{\wedge }\right]}_G^{\vee }=\eta \in {\mathbb{R}}^d$. The middle plot of Figure 1 gives a more intuitive illustration.

Matrix Lie exponential and logarithm maps: Matrix Lie exponential map ${exp}_G:{\mathbb{R}}^d\to G$ and its inverse map ${log}_G:G\to {\mathbb{R}}^d$ (the matrix Lie logarithm map) give two bijections between a neighborhood of $0\in {\mathbb{R}}^d$ and a neighborhood of group identity matrix ${\mathit{I}}_{d\times d}\in G$. We define the matrix Lie exponential and logarithm maps as ${exp}_G(·):={exp}_m\left({[·]}_G^{\wedge }\right)$ and ${log}_G(·):={\left[{log}_m(·)\right]}_G^{\vee }$, respectively, where ${\exp }_m$ and ${\log }_m$ are matrix exponential and logarithm operations. Here, matrix exponential and logarithm operations present the link between a Lie group and its unique corresponding Lie algebra: ${exp}_m:\mathfrak{g}\to G$ and ${log}_m:G\to \mathfrak{g}$. For vector $\eta \in {\mathbb{R}}^d$, we have ${exp}_G\left(\eta \right)={exp}_m\left({\eta }^{\wedge }\right)$. All maps among matrix Lie group G, Lie algebra $\mathfrak{g}$, and vector space ${\mathbb{R}}^d$ are illustrated in detail on the right plot of Figure 1.

2.2. Uncertainty on Matrix Lie Groups

Most system noise is assumed to be additive white Gaussian noise in Euclidean spaces. However, the approach of additive Gaussian noise is not applicable to Lie groups, as Lie groups lack the addition operation. To describe Gaussian uncertainties for general Lie groups, we adopted the method in [15,16,34], which first assumes the distribution of uncertainty in the Lie algebra to be Gaussian and then maps Gaussian distribution to the corresponding Lie group by exponential mapping.

Consider a d-dimension random variable $\chi$ evolving on G, and let the variable satisfy prior probability distribution $\chi \sim {\mathcal{N}}_{\varphi }(\widehat{\chi },\mathit{P})$. Referring to the definition of the probability distribution for a random variable on a manifold in [15], we give the definition of probability distribution for $\chi \in G$ as follows:

$$\chi =\varphi \left(\widehat{\chi },\eta \right),\eta \sim \mathcal{N}(0,\mathit{P}),$$

(1)

where $\varphi :G\times {\mathbb{R}}^d\to G$ is a smooth function selected according to the composition of variable $\chi$ that must satisfy $\varphi (\widehat{\chi },0)=\widehat{\chi }$, also named “retraction”; $\widehat{\chi }\in G$ is noise-free and represents the mean of random variable $\chi$; operator $\mathcal{N}(·,·)$ denotes Gaussian distribution in a Euclidean space; $\mathit{P}\in {\mathbb{R}}^{d\times d}$ is the error covariance matrix associated with uncertainty perturbation $\eta$; and $\eta \in {\mathbb{R}}^d$ is a random Gaussian vector in a Euclidean space. Moreover, distribution ${\mathcal{N}}_{\varphi }(·,·)$ is not Gaussian.

The specific operation of function $\varphi (·,·)$ is generally determined by the system’s geometric structure. Left and right Lie group multiplications, and the matrix Lie group exponential are essential parts of $\varphi (·,·)$ no matter the system geometry. Here, with right and left multiplication, we give two simplified definitions of $\varphi (\widehat{\chi },\eta )$, denoted by ${\varphi }_R(\widehat{\chi },\eta )={exp}_G\left(\eta \right)\widehat{\chi }$ and ${\varphi }_L(\widehat{\chi },\eta )=\widehat{\chi }{exp}_G\left(\eta \right)$. The components of the variable evolving on G are different, so the expression of function $\varphi (·,·)$ is different in various system dynamics and applications.

3. Bayesian Estimation Based on Bayes–Sard Quadrature Moment Transform

This section aims to find a method to calibrate numerical quadrature errors in the classical sigma point filtering on Lie groups. By taking the cubature Kalman filter into consideration and adopting the BSQMT instead of the cubature transform, we propose a Bayesian estimation method with a generalized measurement model.

3.1. Bayes–Sard Quadrature Moment Transform with Cubature Points

The BSQMT [11] is a universal moment transform proposed to accommodate improper estimation and prediction calibration caused by quadrature error in sigma point filters. The main technique of the BSQMT is the Bayesian quadrature, which treats the numerical process as probabilistic inference, and models the integration error by utilizing stochastic process models. Consider nonlinear vector function

$$\mathit{z}=g\left(\mathit{x}\right),$$

(2)

where input $\mathit{x}\in {\mathbb{R}}^d$ is a Gaussian random variable that satisfies Gaussian distribution $\mathcal{N}(\overline{\mathit{x}},\mathit{P})$; $\mathit{z}\in {\mathbb{R}}^l$ is the output; $g(·):{\mathbb{R}}^d\to {\mathbb{R}}^l$ denotes a known nonlinear transformation function. A brief generalization of the BSQMT algorithm with cubature points is given as follows:

Step 1. Assume that the mean $\overline{\mathit{x}}$ and covariance $\mathit{P}$ of random input variable $\mathit{x}$ are known. Calculate third-degree cubature points that match the transform moment of random variable

$${\mathit{X}}_i=\overline{\mathit{x}}+\mathit{S}{\xi }_i,i=1,\dots ,2d,$$

(3)

where $\mathit{S}$ represents the lower triangular factor in the Cholesky decomposition of $\mathit{P}$; ${\xi }_i$ denotes unit cubature points expressed as

$${\xi }_i=\left\{\begin{array}{c}\sqrt{d}{\mathit{e}}_i,i=1,\dots ,d\hfill \\ -\sqrt{d}{\mathit{e}}_{i-d},i=d+1,\dots ,2d\hfill \end{array}\right.,$$

(4)

where ${\mathit{e}}_i\in {\mathbb{R}}^d$ is the i-th column of identity matrix ${\mathit{I}}_{d\times d}$.

Step 2. Compute the quadrature weight: BSQMT uses a nonzero mean hierarchical Gaussian process (GP) prior model, and Bayes–Sard weights merely depend on the choice of sigma points and basic function space $\pi :=span\left\{{\psi }_1,\dots ,{\psi }_N\right\}$. Let $\psi \left(x\right)=x^{\alpha },x\in {\mathbb{R}}^d$, where $\alpha$ is multi-index; then, $x^{\alpha }=x_1^{{\alpha }_1}\cdots x_d^{{\alpha }_d}$ and $\left|\alpha \right|=|{\alpha }_1+\cdots +{\alpha }_d|$. Here, we consider a special base function space in which $N=2d$ holds.

Lemma 1.

Let $p\left(\mathit{x}\right)=\mathcal{N}(\mathit{x}\mid 0,I)$ be standard normal distribution, choose $2d$ cubature points by (4), and construct $2d$ dimensional function space

$$\pi =\left\{{\mathit{x}}_1,\dots ,{\mathit{x}}_d,{\mathit{x}}_1^2,\dots ,{\mathit{x}}_d^2\right\}.$$

(5)

Then, a set of quadrature weights $\mathit{w}=\left\{w_m,n=1,\dots ,2d\right\}$ can decide the weighted mean of the state variable, and quadrature weights cohere with cubature transform weights $w_m=1/2d$.

Proof (Proof of Lemma 1).

See Appendix A.    □

With third-degree unit cubature points, quadrature weights are calculated with the following equations:

$$\left\{\begin{array}{c}{w}_m={\mathsf{\Psi }}^{-\mathrm{T}}\overline{\psi }\hfill \\ w={\mathsf{\Psi }}^{-\mathrm{T}}{W}_A{\mathsf{\Psi }}^{-1}\hfill \\ w_c=W_B{\mathsf{\Psi }}^{-1}\hfill \end{array}\right.,$$

(6)

in which

$$\left\{\begin{array}{c}\overline{\psi }={\mathrm{E}}_{\xi }\left[\psi \left(\xi \right)\right]\hfill \\ \mathsf{\Psi }={\left[\psi \left({\xi }_1\right),\dots ,\psi \left({\xi }_{2d}\right)\right]}^{\mathrm{T}}\hfill \\ W_A={\mathrm{E}}_{\xi }\left[\psi \left(\xi \right)\psi {\left(\xi \right)}^{\mathrm{T}}\right]\hfill \\ W_B={\mathrm{E}}_{\xi }\left[\xi \psi {\left(\xi \right)}^{\mathrm{T}}\right]\hfill \end{array}\right..$$

(7)

From Lemma 1, we know that $w_m=1/2d$. Thus, we can skip the calculation of $w_m$ in (6).

Step 3. Obtain transformed moments with the following equations:

$$\begin{array}{ccc}& & {\mathit{m}}_z=\mathit{Z}{w}_m,\hfill \end{array}$$

(8)

$$\begin{array}{ccc}& & {\mathit{P}}_{zz}=\mathit{Z}w{\mathit{Z}}^{\mathrm{T}}-{\mathit{m}}_z{\mathit{m}}_z^{\mathrm{T}}+{\overline{\sigma }}^2\mathit{I},\hfill \end{array}$$

(9)

$$\begin{array}{ccc}& & {\mathit{C}}_{xz}=\mathit{S}{w}_c{\mathit{Z}}^{\mathrm{T}},\hfill \end{array}$$

(10)

where $\mathit{Z}=\left[g\left({\mathit{X}}_1\right),\dots ,g\left({\mathit{X}}_{2d}\right)\right]$. ${\overline{\sigma }}^2$ is expected model variance (EMV) of which the value partly depends on the selection of radial basis function (RBF) kernel (covariance of GP model based on unit cubature points) and its parameter. However, in practice, EMV sometimes needs to be reparameterized, but we omitted calculating it here. See [11,38] for the detailed computation of EMV and the selection rules of the basic function.

3.2. Bayesian Estimation on Lie Groups Using BSQMT

In this subsection, we first give a generalized measurement model covering system measurements in two kinds of spaces: Euclidean space and Lie group. Consider random variable $\chi \in G$ and its prior probability density function $p\left(\chi \right)$, $\chi$ follows (1) with known $\widehat{\chi }$ and $\mathit{P}$. Assume that one can establish additional information about the random variable via an observation y:

$$\mathit{y}=h_a(\chi ,\mathit{v})=\left\{\begin{array}{cc}{h}_{a1}\left(\chi \right)+\mathit{v},\hfill & \mathrm{for}\mathit{y}\in {\mathbb{R}}^l\hfill \\ \varphi (\chi ,\mathit{v}),\hfill & \mathrm{for}\mathit{y}\in G\hfill \end{array}\right.,$$

(11)

where $\mathit{v}$ is white Gaussian noise with distribution $\mathit{v}\sim \mathcal{N}\left(0,\mathit{R}\right)$ in the vector space, and its dimension depends on the type of $\mathit{y}$; $h_a(·)$ and $h_{a1}(·)$ represent the observation function, and $h_a(·)$ satisfies the following mapping:

$$h_a(·):\left\{\begin{array}{cc}G\to {\mathbb{R}}^l,\hfill & \mathrm{for}\mathit{y}\in {\mathbb{R}}^l\hfill \\ G\times {\mathbb{R}}^d\to G,\hfill & \mathrm{for}\mathit{y}\in G\hfill \end{array}\right.$$

(12)

The key to addressing the Bayesian estimation problem is to find parameters ${\widehat{\chi }}^{+}$ and ${\mathit{P}}^{+}$ in posterior distribution approximated by $p(\chi \mid \mathit{y})\approx {\mathcal{N}}_{\varphi }\left({\widehat{\chi }}^{+},{\eta }^{+}\right),{\eta }^{+}\sim \mathcal{N}\left(0,{\mathit{P}}^{+}\right)$. Before estimating ${\widehat{\chi }}^{+}$, posterior distribution $p(\eta \mid \mathit{y})$ should be found and approximated using BSQMT. Whether the measurement is in a vector space or a Lie group, there is an achievable way to calculate posterior estimate ${\eta }^{+}$ in the vector space and then map it to the Lie group. Hence, while an available measurement $\mathit{y}$ exists in a Lie group, we need to transform the matrix Lie group measurement into the vector space by utilizing the inverse mapping of $\varphi (·,·)$. Inverse map ${\varphi }^{-1}(·,·):G\to {\mathbb{R}}^d$ is defined as

$${\varphi }^{-1}(\widehat{\chi },\mathit{y})=\left\{\begin{array}{c}{\varphi }_L^{-1}(\widehat{\chi },\mathit{y})={log}_G\left({\widehat{\chi }}^{-1}\mathit{y}\right),\mathrm{left}\mathrm{multiplication}\hfill \\ {\varphi }_R^{-1}(\widehat{\chi },\mathit{y})={log}_G\left(\mathit{y}{\widehat{\chi }}^{-1}\right),\mathrm{right}\mathrm{multiplication}\hfill \end{array}\right..$$

(13)

In terms of left and right multiplications in both $\varphi (·,·)$ and ${\varphi }^{-1}(·,·)$, if $\mathit{y}$ exists in a Lie group, there are actually four possible expansions to map it to the vector space. For instance, similar to [16], suppose measurement function $\mathit{y}=\chi {exp}_G\left(\mathit{v}\right)$, if $\chi =\widehat{\chi }{exp}_G\left(\eta \right)$,

$$\begin{array}{cc}\hfill {\mathit{y}}_L& ={\varphi }_L^{-1}(\widehat{\chi },\mathit{y})={log}_G\left({\widehat{\chi }}^{-1}\chi {exp}_G\left(\mathit{v}\right)\right)\hfill \\ \hfill & ={log}_G\left({\widehat{\chi }}^{-1}\widehat{\chi }{exp}_G\left(\eta \right){exp}_G\left(\mathit{v}\right)\right)\hfill \\ \hfill & ={log}_G\left({exp}_G\left(\eta \right){exp}_G\left(\mathit{v}\right)\right)\hfill \end{array},$$

(14)

else if $\chi ={exp}_G\left(\eta \right)\widehat{\chi }$, there is

$$\begin{array}{cc}\hfill {\mathit{y}}_L& ={\varphi }_L^{-1}(\widehat{\chi },\mathit{y})={log}_G\left({\widehat{\chi }}^{-1}\chi {exp}_G\left(\mathit{v}\right)\right)\hfill \\ \hfill & ={log}_G\left({\widehat{\chi }}^{-1}{exp}_G\left(\eta \right)\widehat{\chi }{exp}_G\left(\mathit{v}\right)\right)\hfill \\ \hfill & ={log}_G\left({exp}_G\left(A{d}_{{\widehat{\chi }}^{-1}}\left(\eta \right)\right){exp}_G\left(\mathit{v}\right)\right)\hfill \end{array},$$

(15)

where $Ad(·)$ is the joint operator of a Lie group, there is $A{d}_{{\widehat{\chi }}^{-1}}\left(\eta \right)={\widehat{\chi }}^{-1}\eta \widehat{\chi }$.

In addition, when an available measurement $\mathit{y}$ is in a vector space, it fulfils the following equation:

$$\mathit{y}=h_{a1}\left(\chi \right)+\mathit{v}=h_{a1}\left(\varphi (\widehat{\chi },\eta )\right)+\mathit{v}.$$

(16)

From (14)–(16), we can detect that the optimized measurement function is related to $\widehat{\chi }$, $\eta$ and $\mathit{v}$. So, giving a novel measurement equation for the derivation of the generalized nonlinear filtering:

$${\mathit{y}}^{\prime }=h(\widehat{\chi },\eta ,\mathit{v}),$$

(17)

where ${\mathit{y}}^{\prime }$ is the new observation, and $h(·)$ is the new generalized measurement function.

By using BSQMT, propagated measurement cubature points and the measurement mean are computed:

$$\left\{\begin{array}{c}{\widehat{\mathit{y}}}_i^{\prime }=h\left(\widehat{\chi },{\eta }_i,0\right),i=1,\dots ,2d\hfill \\ {\overline{\mathit{y}}}^{\prime }=\mathit{Y}{w}_m\hfill \end{array}\right.,$$

(18)

where ${\eta }_i$ is cubature points; $\mathit{Y}=\left[{\widehat{\mathit{y}}}_1^{\prime },\dots ,{\widehat{\mathit{y}}}_{2d}^{\prime }\right],i=1,\dots ,2d$ and $w_m$ is the mean weight defined in the above subsection. Afterwards, by calculating measurement error covariance ${\mathit{P}}_{yy}$ and cross-covariance ${\mathit{P}}_{\eta y}$, we obtain gain matrix K and parameters in the approximation of posterior distribution $p(\eta \mid \mathit{y})\sim \mathcal{N}\left(\widehat{\eta },{\mathit{P}}^{+}\right):$

$$\left\{\begin{array}{c}\mathit{K}={\mathit{P}}_{\eta y}{\mathit{P}}_{yy}^{-1}\hfill \\ {\mathit{P}}^{+}=\mathit{P}-\mathit{K}{\mathit{P}}_{yy}{\mathit{K}}^{\mathrm{T}}\hfill \\ \widehat{\eta }=\mathit{K}\left({\mathit{y}}^{\prime }-{\overline{\mathit{y}}}^{\prime }\right)\hfill \end{array}\right..$$

(19)

For converting posterior distribution $p(\eta \mid \mathit{y})$ into a distribution on the Lie group, we reviewed analyses and results in [15,16] and obtained

$${\widehat{\chi }}^{+}=\varphi (\widehat{\chi },\widehat{\eta })=\varphi \left(\widehat{\chi },\mathit{K}\left({\mathit{y}}^{\prime }-{\overline{\mathit{y}}}^{\prime }\right)\right).$$

(20)

Lastly, the entire Bayesian estimation procedure is summarized in Algorithm 1.

Algorithm 1 Bayesian estimation on Lie groups using BSQMT.

Input:  prior state $\widehat{\chi }$ and prior covariance $\mathit{P}$, unit cubature points ${\xi }_i,i=1,2,\dots ,2d$, sequence of observations $\mathit{y}$, modified measurement noise covariance $\widehat{\mathit{R}}$, Bayes–Sard weights $w_m,w,w_c$, expected model variance ${\overline{\sigma }}_h^2$ (calculated by the method in [11]) Output:  posterior state ${\widehat{\chi }}^{+}$ and associated posterior covariance ${\mathit{P}}^{+}$ 1: Calculate predicted measurement, and its associated cross-covariance and innovation covariance: $$\begin{array}{c}{\eta }_i=chol\left(\mathit{P}\right){\xi }_i,i=1,\dots ,2d\hfill \\ {\widehat{\mathit{y}}}_i^{\prime }=h\left(\varphi \left(\widehat{\chi },{\eta }_i,0\right)\right),i=1,\dots ,2d\hfill \\ {\overline{\mathit{y}}}^{\prime }=\mathit{Y}{w}_m,\mathrm{in}\mathrm{which}\mathit{Y}=\left[{\widehat{\mathit{y}}}_1^{\prime },\dots ,{\widehat{\mathit{y}}}_{2d}^{\prime }\right],i=1,\dots ,2d\hfill \\ {\mathit{P}}_{yy}=\mathit{Y}w{\mathit{Y}}^{\mathrm{T}}-{\overline{\mathit{y}}}^{\prime }{\overline{\mathit{y}}}^{\prime \mathrm{T}}+{\overline{\sigma }}_h^2\mathit{I}+\widehat{\mathit{R}}\hfill \\ {\mathit{P}}_{\eta y}=chol\left(\mathit{P}\right)w_c{\mathit{Y}}^{\mathrm{T}}\hfill \end{array}$$ 2: Calculate the gain matrix: $\mathit{K}={\mathit{P}}_{\eta y}{\mathit{P}}_{yy}^{-1}$; 3: Compute the estimated state and covariance: $$\begin{array}{c}{\widehat{\chi }}^{+}=\varphi \left(\widehat{\chi },\mathit{K}\left({\mathit{y}}^{\prime }-{\overline{\mathit{y}}}^{\prime }\right)\right),\mathrm{where}{\mathit{y}}^{\prime }=\left\{\begin{array}{c}\mathit{y},\mathrm{for}\mathit{y}\in {\mathbb{R}}^l\hfill \\ {\varphi }^{-1}(\widehat{\chi },\mathit{y}),\mathrm{for}\mathit{y}\in G\hfill \end{array}\right.\hfill \\ {\mathit{P}}^{+}=\mathit{P}-\mathit{K}{\mathit{P}}_{yy}{\mathit{K}}^{\mathrm{T}}\hfill \end{array}$$

4. Proposed Filtering Algorithm

This section presents a Bayes–Sard cubature Kalman filter on matrix Lie groups built on numerical integration error calibration with BSQMT to correct the filtering accuracy and named BSCKF-LG. The proposed BSCKF-LG has two versions: one version uses left multiplication to define “retraction” as left BSCKF-LG; the other uses right multiplication defined as right BSCKF-LG. A systemic method that leverages the maximum likelihood criterion is also proposed to estimate the EMVs. By reconstructing the covariances of the prior state error and the innovation with the innovation sequence in a sliding window, the proposed method estimates EMVs and feeds them back into improved filtering to adjust the Lie Kalman gain matrix.

4.1. Improved Cubature Kalman Filtering on Lie Groups with BSQMT

Consider discrete system dynamics

$$\left\{\begin{array}{c}{\chi }_k=f\left({\chi }_{k-1},{\omega }_k,{\mathit{n}}_k\right)\hfill \\ {\mathit{y}}_k=h_a\left({\chi }_k,{\mathit{v}}_k\right)\hfill \end{array}\right.,$$

(21)

where k represents the timestamp; ${\chi }_k$ is the system state living in a matrix Lie group; ${\mathit{n}}_k\sim \mathcal{N}\left(0,{\mathit{Q}}_k\right)$ and ${\mathit{v}}_k\sim \mathcal{N}\left(0,{\mathit{R}}_k\right)$ are white Gaussian noise with known covariances in the vector space; and ${\omega }_k$ is a known input. According to the above section, we first needed to remodel measurement ${\mathit{y}}_k$ into ${\mathit{y}}_k^{\prime }$, and ${\mathit{y}}_k^{\prime }$ is in the Euclidean vector space.

The purpose of designing filtering in the Bayesian framework of this model is to approximate posterior distribution $p\left({\chi }_k,{\mathit{P}}_k\mid {\mathit{y}}_{\mathrm{l}:k}^{\prime }\right)$. The recursive solution comprises two steps to the proposed filtering problem on Lie groups: propagation and update.

Propagation: start with giving prior probability destiny in the Bayesian framework.

$$p\left({\chi }_{k-1}\right)\approx {\mathcal{N}}_{\varphi }\left({\widehat{\chi }}_{k-1}^{+},{\eta }_{k-1}^{+}\right),{\eta }_{k-1}^{+}\sim \mathcal{N}\left(0,{\mathit{P}}_{k-1}^{+}\right),$$

(22)

where ${\widehat{\chi }}_{k-1}^{+}$ and ${\mathit{P}}_{k-1}^{+}$ are posterior estimation results of the previous moment. Then, try to approximate the state distribution of propagation:

$$p\left({\chi }_k\mid {\mathit{y}}_{1:k-1}^{\prime }\right)\approx {\mathcal{N}}_{\varphi }\left({\widehat{\chi }}_k,{\eta }_k\right),{\eta }_k\sim \mathcal{N}\left(0,{\mathit{P}}_k\right).$$

(23)

Our work needs to find ${\widehat{\chi }}_k$ and ${\mathit{P}}_k$. In the above Bayesian estimation, posterior state mean ${\widehat{\chi }}_k^{+}$ and its associated covariance $P_k^{+}$ are estimated using the uncertainty represented by (1). Here, for Model (21), we still used this uncertainty representation and the associated inverse map that has

$${\varphi }^{-1}\left({\widehat{\chi }}_k,{\chi }_k\right)={\varphi }^{-1}\left({\widehat{\chi }}_k,\varphi \left({\widehat{\chi }}_k,{\eta }_k\right)\right)=log\left(exp\left({\eta }_k\right)\right)={\eta }_k+O\left({∥{\eta }_k∥}^2\right),$$

(24)

where $O\left({∥{\eta }_k∥}^2\right)$ stands for error term. The proposed filter first propagates state mean ${\widehat{\chi }}_k$ via employing the system’s deterministic part (noiseless state model), that is,

$${\widehat{\chi }}_k=f\left({\widehat{\chi }}_{k-1}^{+},{\omega }_k,0\right).$$

(25)

Subsequently, to compute associated covariance ${\mathit{P}}_k$, we selected a set of unit cubature points for the BSQMT method and generated sigma points ${\eta }_{k-1,i}^{+}$ in vector space ${\mathbb{R}}^d$ through ${\mathit{P}}_{k-1}^{+}$. Then, mapping these points to the matrix Lie group and passing the mapped cubature points through noise-free Model (25), we could obtain propagated sigma points ${\chi }_k^i$. On the basis of this work and with defined inverse map ${\varphi }^{-1}\left({\widehat{\chi }}_k,{\chi }_k^i\right),i=1,\dots ,2d$, we obtained prior covariance ${\mathit{P}}_k$ (see Algorithm 2 for details).

Update: in this procedure, we need to approximate posterior probability distribution.

$$p\left({\chi }_k\mid {\mathit{y}}_{1:k}^{\prime }\right)\approx {\mathcal{N}}_{\varphi }\left({\widehat{\chi }}_k^{+},{\eta }_k^{+}\right),{\eta }_{\widehat{k}}^{+}\sim \mathcal{N}\left(0,{\mathit{P}}_k^{+}\right),$$

(26)

when each available measurement ${\mathit{y}}_k$ arrives. Due to the same measurement model, the Bayesian estimation method in Section 3 was applied to compute the posterior state and its associated covariance. The proposed filtering algorithm executes the propagation step and does not perform the update step until the measurement arrives. Algorithm 2 summarizes the proposed filter in detail.

Algorithm 2 Bayes–Sard cubature Kalman filter (BSCKF-LG).

Input:  initial conditions ${\chi }_0$ and ${\mathit{P}}_{0\mid 0}$, sequence of inputs ${\omega }_k,k=1,2,\dots$, modified process and measurement noise covariances ${\widehat{\mathit{Q}}}_k$ and ${\widehat{\mathit{R}}}_k$, sequence of observations ${\mathit{y}}_k,k=1,2,\dots$, RBF kernel parameters ${\theta }_f$ and ${\theta }_h$ (for computing EMV), unit cubature points ${\xi }_i,i=1,2,\dots ,2d$ Output:  posterior state ${\widehat{\chi }}_k^{+}$, posterior covariance ${\mathit{P}}_k^{+}$ 1: Initialization: ${\widehat{\chi }}_0^{+}={\chi }_0$ and ${\mathit{P}}_0^{+}={\mathit{P}}_{0\mid 0}$; 2: Compute Baye–Sard quadrature weights $w_m$, w, and $w_c$ from (6) and (7), and calculate the EMV ${\overline{\sigma }}_f^2$ with the method in [11]; 3: for$k=1,2,3,\dots$do Propagation: 4:     Calculate state mean through noiseless system model $${\widehat{\chi }}_k=f\left({\widehat{\chi }}_{k-1}^{+},{\omega }_k,0\right)$$ 5:     Generate propagated sigma points ${\chi }_k^i$ on the basis of selected unit cubature points according to (4), $$\begin{array}{c}{\eta }_{k-1,i}^{+}=chol\left({\mathit{P}}_{\dot{k}-1}^{+}\right){\xi }_i,i=1,\dots ,2d\hfill \\ {\chi }_k^i=f\left(\varphi \left({\widehat{\chi }}_{k-1}^{+},{\eta }_{k-1,i}^{+}\right),{\omega }_k,0\right),i=1,\dots ,2d\hfill \end{array}$$ 6:     Calculate associated covariance according to inverse map ${\varphi }^{-1}\left({\widehat{\chi }}_k,{\chi }_k^i\right),i=1,\dots ,2d$, $$\begin{array}{c}\hfill {\mathit{P}}_k={\mathsf{\Phi }}_{k}w{\mathsf{\Phi }}_k^{\mathrm{T}}+{\overline{\sigma }}_f^{2}I+{\widehat{\mathit{Q}}}_k\end{array}$$       where ${\mathsf{\Phi }}_k=\left[{\varphi }^{-1}\left({\widehat{\chi }}_k,{\chi }_k^1\right),\dots ,{\varphi }^{-1}\left({\widehat{\chi }}_k,{\chi }_k^{2d}\right)\right]$, ${\widehat{\mathit{Q}}}_k$ derived from the dynamical equation of ${\eta }_k$; Update: (If the measurement ${\mathit{y}}_k$ arrives, start the update step of the filtering. BSQ weights and EMV value are known. ) 7:     Compute ${\widehat{\chi }}_k^{+},{\mathit{P}}_k^{+}$ from Algorithm 1 with ${\widehat{\chi }}_k,{\mathit{P}}_k$ 8: end for

Remark 1.

The propagation of state means does not need to utilize the BSQMT(or other moment transformation) because (25) was validated by [16] up to the second order. Instead of generating the sigma points directly by the distribution on the Lie group, all work about generating the corresponding sigma points is first done in the Euclidean space and then mapped to the Lie group.

Remark 2.

The unscented Kalman filters on Lie groups in [15,16,17] generate sigma points for the process noise and calculate the covariance matrix of the deviation due to noise. Unlike these filters, process and measurement noise covariances${\widehat{\mathit{Q}}}_k$and${\widehat{\mathit{R}}}_k$in Algorithm 2 are determined by the specified dynamic equation of${\eta }_{\mathit{k}}$and the remodeled measurement function of${\mathit{y}}_k^{\prime }$to save computing resources.

Remark 3.

BSQMT rules define all Bayes–Sard weights. Weights only depend on the selection of the sigma points and the basic function. The proposed filter requires two different kernel parameter values to obtain EMV values (see [11] for details) because there are two integrated functions. The kernel parameter’s misspecification affects the BSQMT little, so manual parameter tuning of the EMV is available in practice. In the following section, we give a method to obtain the time-varying value of EMV.

4.2. EMV Estimation

In this subsection, we attempt to develop an adaptive BSCKF-LG with time-varying EMVs. To derive the EMV at each instant k, we first assumed that the prior covariance and the innovation covariance are unknown constants. The innovation vector is denoted as ${\tilde{\mathit{y}}}_k^{\prime }={\mathit{y}}_k^{\prime }-{\overline{\mathit{y}}}_k^{\prime }$, and we consider the following maximum likelihood function of the rebuild measurements $\left\{{\mathit{y}}_{k-n+1}^{\prime },{\mathit{y}}_{k-n+2}^{\prime },\dots ,{\mathit{y}}_k^{\prime }\right\}$:

$$\begin{array}{cc}\hfill & L\left({\mathit{P}}_{yy,k}\left(\mathrm{or}{\mathit{P}}_k\right)\mid {\mathit{y}}_{k-n+1:k}^{\prime }\right)=ln\prod _{j=k-n+1}^{k}p\left({\mathit{y}}_j^{\prime }\mid {\mathit{P}}_{yy,j}\left(\mathrm{or}{\mathit{P}}_j\right)\right)\hfill \\ \hfill & =ln\prod _{j=k-n+1}^k\frac{1}{\sqrt{{\left(2\pi \right)}^l\left|{\mathit{P}}_{yy,j}\right|}}exp\left(-\frac{1}{2}{\tilde{\mathit{y}}}_j^{\prime \mathrm{T}}{\mathit{P}}_{yy,j}^{-1}{\tilde{\mathit{y}}}_j^{\prime }\right)\hfill \\ \hfill & =-\frac{1}{2}\sum _{j=k-n+1}^k\left\{ln\left(\left|{\mathit{P}}_{yy,j}\right|\right)+{\tilde{\mathit{y}}}_j^{\prime \mathrm{T}}{\mathit{P}}_{yy,j}^{-1}{\tilde{\mathit{y}}}_j^{\prime }\right\}+C\hfill \end{array},$$

(27)

where $|·|$ denotes the determinant operator, n represents the sliding window size, and C is the constant term. On the basis of (27), the maximum likelihood estimation of the error covariances of the prior state and the prior measurement is derived by

$${\widehat{\mathit{P}}}_{yy,k\mid ML}\left({\widehat{\mathit{P}}}_{k\mid ML}\right)=argmaxL\left({\mathit{P}}_{yy,k}\left(\mathrm{or}{\mathit{P}}_k\right)\mid {\mathit{y}}_{k-n+1:k}^{\prime }\right).$$

(28)

To solve (28), we need to take the partial derivatives of $L\left({\mathit{P}}_{yy,k}\left(\mathrm{or}{\mathit{P}}_k\right)\mid {\mathit{y}}_{k-n+1:k}^{\prime }\right)$ with respect to estimated prior covariance ${\widehat{\mathit{P}}}_k$ and estimated innovation covariance ${\widehat{\mathit{P}}}_{yy,k}$, and let them be equal to zero:

$$\frac{\partial L}{\partial {\widehat{\mathit{P}}}_{yy,k}}=0\mathrm{and}\frac{\partial L}{\partial {\widehat{\mathit{P}}}_k}=0.$$

(29)

Subsequently, the maximum likelihood equations are obtained via Equation (29):

$$\frac{\partial L}{\partial {\widehat{\mathit{P}}}_{yy,k}}=-\frac{1}{2}tr\left\{\sum _{j=k-n+1}^k\left[\left({\mathit{P}}_{yy,j}^{-1}-{\mathit{P}}_{yy,j}^{-1}{\tilde{\mathit{y}}}_j^{\prime }{\tilde{\mathit{y}}}_j^{\prime \mathrm{T}}{\mathit{P}}_{yy,j}^{-1}\right)\frac{\partial {\mathit{P}}_{yy,j}}{\partial {\widehat{\mathit{P}}}_{yy,k}^{a_1,b_1}}\right]\right\},$$

(30)

and

$$\frac{\partial L}{\partial {\widehat{\mathit{P}}}_k}=-\frac{1}{2}tr\left\{\sum _{j=k-n+1}^k\left[\left({\mathit{P}}_{yy,j}^{-1}-{\mathit{P}}_{yy,j}^{-1}{\tilde{\mathit{y}}}_j^{\prime }{\tilde{\mathit{y}}}_j^{\prime \mathrm{T}}{\mathit{P}}_{yy,j}^{-1}\right)\frac{\partial {\mathit{P}}_{yy,j}}{\partial {\widehat{\mathit{P}}}_k^{a_2,b_2}}\right]\right\},$$

(31)

where $tr(·)$ denotes the trace operator; ${\widehat{\mathit{P}}}_{y,k}^{a_1,b_1}$ corresponds to the $a_1$th row, $b_1$th column of ${\widehat{\mathit{P}}}_{yy,k}$ with $a_1,b_1\in [1,\dots ,l]$; similarly, ${\widehat{\mathit{P}}}_k^{a_2,b_2}$ corresponds to the $a_2$th row, $b_2$th column of ${\widehat{\mathit{P}}}_k$ with $a_2,b_2\in [1,\dots ,d]$.

Equations (29) and (30) show that

$$tr\left\{\sum _{j=k-n+1}^k\left[\left({\mathit{P}}_{yy,j}^{-1}-{\mathit{P}}_{yy,j}^{-1}{\tilde{\mathit{y}}}_j^{\prime }{\tilde{\mathit{y}}}_j^{\prime \mathrm{T}}{\mathit{P}}_{yy,j}^{-1}\right)\frac{\partial {\mathit{P}}_{yy,j}}{\partial {\widehat{\mathit{P}}}_{yy,k}^{a_1,b_1}}\right]\right\}=0.$$

(32)

Therefore, the problem of estimating ${\mathit{P}}_{yy,k}$ is actually computing the derivation of the innovation covariance with respect to ${\mathit{P}}_{yy,j},j=k-n+1,\dots ,k$. As stated above, state errors are mapped from the Lie group into the Euclidean space, and prior state error covariance in the Euclidean space can be expressed as ${\mathit{P}}_k=\mathrm{E}\left[{\eta }_k{\eta }_k^{\mathrm{T}}\right]$. In this article, ${\eta }_k={\varphi }^{-1}\left({\widehat{\chi }}_k,{\chi }_k\right)$ and ${\eta }_k^{+}={\varphi }^{-1}\left({\widehat{\chi }}_k^{+},{\chi }_k\right)$. Using (21) and the formula of ${\mathit{P}}_k$ in Algorithm 2, we first have

$$\begin{array}{cc}\hfill & {\eta }_k={\varphi }^{-1}\left(f\left({\widehat{\chi }}_{k-1}^{+},{\omega }_k\right),f\left({\chi }_{k-1},{\omega }_k,{\mathit{n}}_k\right)\right)\hfill \\ \hfill & ={\varphi }^{-1}\left(f\left({\widehat{\chi }}_{k-1}^{+},{\omega }_k\right),f\left(\varphi \left({\chi }_{k-1}^{+},{\eta }_{k-1}^{+}\right),{\omega }_k,{\mathit{n}}_k\right)\right)\hfill \end{array}.$$

(33)

Then, $f(·)$ and (33) are expressed by the Taylor series expansion about ${\eta }_{k-1}^{+}$, the estimation error ${\eta }_k$ becomes

$${\eta }_k={\mathit{F}}_k{\eta }_{k-1}^{+}+O\left(∥{\eta }_{k-1}^{+}∥\right)+{\widehat{\mathit{n}}}_k,$$

(34)

in which $\mathit{F}=d{\varphi }^{-1}\left(f\left({\widehat{\chi }}_{k-1}^{+},{\omega }_k\right),f\left(\varphi \left({\chi }_{k-1}^{+},{\eta }_{k-1}^{+}\right),{\omega }_k\right)\right)/d{\eta }_{k-1}^{+}$ and $O\left(∥{\eta }_{k-1}^{+}∥\right)$ represent the second- and high-order moments about ${\eta }_{k-1}^{+};{\widehat{\mathit{n}}}_k$ is derived from the specified dynamic equation of ${\eta }_k$, and its covariance is ${\widehat{\mathit{Q}}}_k$. To obtain exact equality, we consider the entire high order of ${\eta }_{k-1}^{+}$ by introducing a diagonal matrix ${\alpha }_k=diag\left({\alpha }_{1,k,\dots ,}{\alpha }_{d,k}\right)$, which can scale the approximation error [39,40]:

$${\eta }_k={\alpha }_k{\mathit{F}}_k{\eta }_{k-1}^{+}+{\widehat{\mathit{n}}}_k.$$

(35)

Similarly, by substituting (18) in ${\tilde{\mathit{y}}}_k^{\prime }={\mathit{y}}_k^{\prime }-{\overline{\mathit{y}}}_k^{\prime }$ and expanding $h(·)$ using the Taylor series about ${\eta }_k$, we have

$${\tilde{\mathit{y}}}_k^{\prime }={\mathit{H}}_k{\eta }_k+O\left(∥{\eta }_k∥\right)+{\widehat{\mathit{v}}}_k,$$

(36)

where ${\mathit{H}}_k=dh\left({\widehat{\chi }}_k,{\eta }_k\right)/d{\eta }_k,O\left(∥{\eta }_k∥\right)$ is the second- and higher-order moment of ${\mathbf{\eta }}_k;{\widehat{\mathit{v}}}_k$ is defined by the remodeled measurement formula, and its covariance is ${\widehat{\mathit{R}}}_k$. To consider the entire higher order, we also introduce a diagonal matrix ${\mathbf{\beta }}_k=diag\left({\beta }_{1,k,\dots ,},{\beta }_{l,k}\right)$ [39]:

$${\tilde{\mathit{y}}}_k{}^{\prime }={\mathbf{\beta }}_k{\mathit{H}}_k{\eta }_k+{\widehat{\mathit{v}}}_k,$$

(37)

Hence, prior state error covariance and innovation covariance can be expressed by

$${\mathit{P}}_k={\mathbf{\alpha }}_k{\mathit{F}}_k{\mathit{P}}_{k-1}^{+}{\mathit{F}}_k^{\mathrm{T}}{\mathbf{\alpha }}_k+{\widehat{\mathit{Q}}}_k,$$

(38)

$${\mathit{P}}_{yy,k}={\mathbf{\beta }}_k{\mathit{H}}_k{\mathit{P}}_k{\mathit{H}}_k^{\mathrm{T}}{\mathbf{\beta }}_k+{\widehat{\mathit{R}}}_k.$$

(39)

Lastly, for linear system models (35) and (37), if the Kalman filtering is steady, ${\mathit{P}}_k$ converges to a constant matrix; ${\mathit{P}}_k^{+},{\mathit{K}}_k$ and ${\mathit{P}}_{yy,k}$ also become constant matrices [41].

On the basis of the above analysis, we assumed that our filtering process inside the sliding window was in a steady state. Consequently, the approximation of ${\mathit{P}}_{yy,k}$ in the sliding window was around constant, and there exists ${\mathit{P}}_{yy,j}={\mathit{P}}_{yy,k},j=k-n+1,\dots ,k$ such that $\partial {\mathit{P}}_{yy,j}/\partial {\widehat{\mathit{P}}}_{yy,k}^{a_1,b_1}=\partial {\mathit{P}}_{yy,k}/\partial {\widehat{\mathit{P}}}_{yy,k}^{a_1,b_1}$ are 1 for the $a_1$ row and $b_1$ column, while other elements are zeros. In light of the trace definition, we simplified (32) to

$$\sum _{j=k-n+1}^k\left({\mathit{P}}_{yy,j}^{-1}-{\mathit{P}}_{yy,j}^{-1}{\tilde{\mathit{y}}}_j^{\prime }{\tilde{\mathit{y}}}_j^{\mathrm{T}}{\mathit{P}}_{yy,j}^{-1}\right)=0.$$

(40)

Since ${\mathit{P}}_{yy,j}$ was approximately constant in the window, we pre- and postmultiplied both sides of (40) with ${\mathit{P}}_{yy,j}$:

$${\widehat{\mathit{P}}}_{yy,k}=\frac{1}{n}\sum _{j=k-n+1}^k{\widehat{\mathit{P}}}_{yy,j}=\frac{1}{n}\sum _{j=k-n+1}^k\left({\tilde{\mathit{y}}}_j^{\prime }{\tilde{\mathit{y}}}_j^{\prime \mathrm{T}}\right).$$

(41)

Substituting ${\mathit{P}}_{yy}=\mathit{Y}w{\mathit{Y}}^{\mathrm{T}}-{\overline{\mathit{y}}}^{\prime }{\overline{\mathit{y}}}^{\prime \mathrm{T}}+{\overline{\sigma }}_h^2\mathit{I}+\widehat{\mathit{R}}$ in (41) and letting ${\Delta }_h={\overline{\sigma }}_h^2\mathit{I}$, the EMV of the measurement function is described as

$${\widehat{\Delta }}_h={\widehat{\overline{\sigma }}}_h^2\mathit{I}=\frac{1}{n}\sum _{j=k-n+1}^k\left[{\tilde{\mathit{y}}}_j^{\prime }{\tilde{\mathit{y}}}_j^{\prime \mathrm{T}}-{\mathit{Y}}_{j}w{\mathit{Y}}_j^{\mathrm{T}}+{\overline{\mathit{y}}}_j^{\prime }{\overline{\mathit{y}}}_j^{\prime \mathrm{T}}-{\widehat{\mathit{R}}}_j\right].$$

(42)

Next, similar steps were employed to estimate the EMV of the system function. Equations (29) and (31) show that

$$\begin{array}{cc}\hfill & tr\left\{\sum _{j=k-n+1}^k\left[\left({\mathit{P}}_{yy,j}^{-1}-{\mathit{P}}_{yy,j}^{-1}{\tilde{\mathit{y}}}_j^{\prime }{\tilde{\mathit{y}}}_j^{\prime \mathrm{T}}{\mathit{P}}_{yy,j}^{-1}\right)\frac{\partial {\mathit{P}}_{yy,j}}{\partial {\widehat{\mathit{P}}}_k^{a_2,b_2}}\right]\right\}\hfill \\ \hfill & =tr\left\{\sum _{j=k-n+1}^k\left[\left({\mathit{P}}_{yy,j}^{-1}-{\mathit{P}}_{yy,j}^{-1}{\tilde{\mathit{y}}}_j^{\prime }{\tilde{\mathit{y}}}_j^{\prime \mathrm{T}}{\mathit{P}}_{yy,j}^{-1}\right){\mathit{H}}_k\frac{\partial {\mathit{P}}_j}{\partial {\widehat{\mathit{P}}}_k^{a_2,b_2}}{\mathit{H}}_k^T\right]\right\}\hfill \\ \hfill & =0\hfill \end{array},$$

(43)

where ${\mathit{H}}_k={\mathit{P}}_{\eta y,k}^{\mathrm{T}}{\mathit{P}}_k^{-1}$ or ${\mathit{H}}_k=d{\mathit{y}}_k^{\prime }/d{\eta }_k$.

Again, since the filtering process was assumed to be stable in the sliding window, approximations of ${\mathit{P}}_k$ in the window were almost constant, ${\mathit{P}}_j={\mathit{P}}_k,j=k-n+1,\dots ,k$, such that $\partial {\mathit{P}}_j/\partial {\widehat{\mathit{P}}}_k^{a_2,b_2}=\partial {\mathit{P}}_k/\partial {\widehat{\mathit{P}}}_k^{a_2,b_2}$ are 1 for the $a_2$ row and $b_2$ column, while other elements are zeros. Equation (43) was pre- and postmultiplied by $H^{\mathrm{T}}$ and $H^{-\mathrm{T}}$; then, in terms of the trace definition, we simplified (43) to

$$\sum _{j=k-n+1}^k\left[{\mathit{H}}_j^{\mathrm{T}}\left({\mathit{P}}_{yy,j}^{-1}-{\mathit{P}}_{yy,j}^{-1}{\tilde{\mathit{y}}}_j^{\prime }{\tilde{\mathit{y}}}_j^{\prime }{}^{\mathrm{T}}{\mathit{P}}_{yy,j}^{-1}\right){\mathit{H}}_j\right]=0.$$

(44)

In sigma point filters, there exists ${\mathit{K}}_k={\mathit{P}}_{\eta y,k}{\mathit{P}}_{yy,k}^{-1}={\mathit{P}}_k{\mathit{H}}_k^{\mathrm{T}}{\mathit{P}}_{yy,k}^{-1}$; thus, (44) can be transformed into

$$\begin{array}{cc}\hfill & \sum _{j=k-n+1}^k\left[{\mathit{P}}_j^{-1}\left({\mathit{K}}_j{\mathit{H}}_j{\mathit{P}}_j-{\mathit{K}}_j{\tilde{\mathit{y}}}_j^{\prime }{\tilde{\mathit{y}}}_j^{\prime \mathrm{T}}{\mathit{K}}_j^{\mathrm{T}}\right){\mathit{P}}_j^{-1}\right]\hfill \\ \hfill & ={\mathit{P}}_j^{-1}\sum _{j-k-n+1}^k\left[\left({\mathit{K}}_j{\mathit{H}}_j{\mathit{P}}_j-{\mathit{K}}_j{\tilde{\mathit{y}}}_j^{\prime }{\tilde{\mathit{y}}}_j^{\prime \mathrm{T}}{\mathit{K}}_j^{\mathrm{T}}\right)\right]{\mathit{P}}_j^{-1}\hfill \\ \hfill & =0\hfill \end{array}.$$

(45)

(45) is satisfied if

$$\sum _{j=k-n+1}^k\left[\left({\mathit{K}}_j{\mathit{H}}_j{\mathit{P}}_j-{\mathit{K}}_j{\tilde{\mathit{y}}}_j^{\prime }{\tilde{\mathit{y}}}_j^{\prime \mathrm{T}}{\mathit{K}}_j^{\mathrm{T}}\right)\right]=0.$$

(46)

From Algorithm 2, we know that

$$\left\{\begin{array}{c}{\mathit{K}}_k{\tilde{\mathit{y}}}_k^{\prime }={\varphi }^{-1}\left({\widehat{\chi }}_k,{\widehat{\chi }}_k^{+}\right)\hfill \\ {\mathit{P}}_k-{\mathit{P}}_k^{+}={\mathit{K}}_k{\mathit{P}}_{yy}{\mathit{K}}_k^{\mathrm{T}}={\mathit{K}}_k{\mathit{H}}_k{\mathit{P}}_k\hfill \end{array}\right..$$

(47)

Substituting (47) into (46), there readily is

$$\sum _{j=k-n+1}^k\left({\mathit{P}}_j-{\mathit{P}}_j^{+}-{\varphi }^{-1}\left({\widehat{\chi }}_k,{\widehat{\chi }}_k^{+}\right){\left({\varphi }^{-1}\left({\widehat{\chi }}_k,{\widehat{\chi }}_k^{+}\right)\right)}^{\mathrm{T}}\right)=0,$$

(48)

then

$$\sum _{j=k-n+1}^k{\mathit{P}}_j=\sum _{j=k-n+1}^k\left({\mathit{P}}_j^{+}+{\varphi }^{-1}\left({\widehat{\chi }}_k,{\widehat{\chi }}_k^{+}\right){\left({\varphi }^{-1}\left({\widehat{\chi }}_k,{\widehat{\chi }}_k^{+}\right)\right)}^{\mathrm{T}}\right),$$

(49)

with ${\mathit{P}}_j={\mathit{P}}_k,j<k$ in the window, the maximum likelihood of prior error covariance ${\mathit{P}}_k$ could be acquired from

$${\widehat{\mathit{P}}}_k=\frac{1}{n}\sum _{j=k-n+1}^k{\mathit{P}}_j=\frac{1}{n}\sum _{j=k-n+1}^k\left({\mathit{P}}_j^{+}+{\varphi }^{-1}\left({\widehat{\chi }}_k,{\widehat{\chi }}_k^{+}\right){\left({\varphi }^{-1}\left({\widehat{\chi }}_k,{\widehat{\chi }}_k^{+}\right)\right)}^{\mathrm{T}}\right).$$

(50)

Combining ${\mathit{P}}_k=\mathsf{\Phi }w{\mathsf{\Phi }}^{\mathrm{T}}+{\overline{\sigma }}_f^2\mathit{I}+{\widehat{\mathit{Q}}}_k$ and (50), and letting ${\Delta }_f={\overline{\sigma }}_f^2\mathit{I}$, the estimated EMV could be described as

$${\widehat{\Delta }}_f={\widehat{\overline{\sigma }}}_f^2\mathit{I}=\frac{1}{n}\sum _{j=k-n+1}^k\left[\begin{array}{c}{\mathit{P}}_j^{+}+{\varphi }^{-1}\left({\widehat{\chi }}_k,{\widehat{\chi }}_k^{+}\right){\left({\varphi }^{-1}\left({\widehat{\chi }}_k,{\widehat{\chi }}_k^{+}\right)\right)}^{\mathrm{T}}\hfill \\ -{\mathsf{\Phi }}_{j}w{\mathsf{\Phi }}_j^{\mathrm{T}}-{\widehat{\mathit{Q}}}_j\hfill \end{array}\right].$$

(51)

Remark 4.

As EMVs must be non-negative$\left({\overline{\sigma }}_f^2\ge 0,{\overline{\sigma }}_h^2\ge 0\right)$and${\Delta }_f,{\Delta }_h$are diagonal matrices, so only estimating the diagonal elements of the EMV matrix is enough. Diagonal elements need to be non-negative; if some diagonal elements are negative, one should set them to zero.

Remark 5.

Equations (42) and (51) show that the validity of EMV estimation depends on process accuracy and noise covariance measurement. Filtering accuracy also relies on noise statistics. In addition, even though (42) and (51) theoretically give adaptive EMVs, they bring computational burdens.

5. Application to Attitude Estimation

In this section, we apply the proposed method to a simulated attitude estimation system and compare it with various filters presented in the cited literature. Consider a rotation rigid body without translation that contains a triaxial gyroscope offering angular velocity ${\omega }_m\in {\mathbb{R}}^3$ at a $100\mathrm{Hz}$ sampling frequency, a triaxial accelerometer providing acceleration ${\mathit{f}}_b\in {\mathbb{R}}^3$ at $10\mathrm{Hz}$ sampling frequency, and a triaxial magnetometer measuring magnetic field ${\mathit{m}}_b\in {\mathbb{R}}^3$ at $10\mathrm{Hz}$ sampling frequency. For the spacecraft attitude estimation problem, one can replace accelerometers and magnetometers with sun sensors and magnetometers [23]. The attitude kinematics in terms of the unit quaternion is described by

$$\left\{\begin{array}{c}\dot{\mathit{q}}=\frac{1}{2}\mathit{q}\otimes \left({\omega }_m-{\mathit{b}}_{\omega }-{\mathit{n}}_{\omega }\right)\hfill \\ {\dot{\mathit{b}}}_{\omega }={\mathit{n}}_{b\omega }\hfill \end{array}\right..$$

(52)

Let accelerometer and magnetometer outputs be observations; observation equations are expressed by [23,42]

$$\left\{\begin{array}{c}{\mathit{f}}_b={\mathit{q}}^{-1}\otimes {\mathit{f}}_n\otimes \mathit{q}+{\mathit{v}}_f\hfill \\ {\mathit{m}}_b={\mathit{q}}^{-1}\otimes {\mathit{m}}_n\otimes \mathit{q}+{\mathit{v}}_m\hfill \end{array}\right.,$$

(53)

in which $\mathit{q}\in SO\left(3\right)$ is the unit quaternion representing the attitude in the body frame with respect to the navigation frame; ${\mathit{b}}_{\omega }\in {\mathbb{R}}^3$ denotes gyroscope bias; ${\mathit{n}}_{\omega }\in {\mathbb{R}}^3$ and ${\mathit{n}}_{b\omega }\in {\mathbb{R}}^3$ are uncorrelated white Gaussian noise; ${\mathit{f}}_n\in {\mathbb{R}}^3$ is the gravity vector in the navigation frame; and ${\mathit{m}}_n\in {\mathbb{R}}^3$ represents Earth’s magnetic field; and ${\mathit{v}}_f\in {\mathbb{R}}^3$ and ${\mathit{v}}_m\in {\mathbb{R}}^3$ are sensor noises assumed to be white Gaussian noise. Let $\chi =(\mathit{q},{\mathit{b}}_{\omega })$, which belongs to matrix Lie group $G=SO\left(3\right)\times {\mathbb{R}}^3$, and with the definition of $\varphi (\widehat{\chi },\eta )$ in Section 2, we define the left and right estimation errors as follows:

$$\left(\begin{array}{c}{\tilde{\mathit{q}}}_R\\ {\tilde{\mathit{b}}}_R\end{array}\right)={exp}_G\left(-{\eta }_R\right)=\left(\begin{array}{c}\widehat{\mathit{q}}\otimes {\mathit{q}}^{-1}\hfill \\ \mathit{q}\otimes \left({\widehat{\mathit{b}}}_{\omega }-{\mathit{b}}_{\omega }\right)\otimes {\mathit{q}}^{-1}\hfill \end{array}\right)(\mathrm{right}\mathrm{estimation}\mathrm{errors})$$

(54)

$$\left(\begin{array}{c}{\tilde{\mathit{q}}}_L\\ {\tilde{\mathit{b}}}_L\end{array}\right)={exp}_G\left({\eta }_L\right)=\left(\begin{array}{c}{\widehat{\mathit{q}}}^{-1}\otimes \mathit{q}\hfill \\ {\mathit{b}}_{\omega }-{\widehat{\mathit{b}}}_{\omega }\hfill \end{array}\right)(\mathrm{left}\mathrm{estimation}\mathrm{errors})$$

(55)

Then, retractions corresponding with the above estimation errors are

$$\begin{array}{c}\hfill {\varphi }_R\left(\widehat{\chi },-{\eta }_R\right)={exp}_G\left(-{\eta }_R\right)\widehat{\chi }=\left(\begin{array}{c}{exp}_q\left(-{\eta }_{R_{1:3}}\right)\otimes \widehat{\mathit{q}}\hfill \\ {\mathit{q}}^{-1}\otimes \left({\widehat{b}}_{\omega }-{\eta }_{R_{4:6}}\right)\otimes \mathit{q}\hfill \end{array}\right)\end{array}$$

(56)

$${\varphi }_L\left(\widehat{\chi },{\eta }_L\right)=\widehat{\chi }{exp}_G\left({\eta }_L\right)=\left(\begin{array}{c}\widehat{\mathit{q}}\otimes {exp}_q\left({\eta }_{L_{1:3}}\right)\\ {\widehat{\mathit{b}}}_{\omega }+{\eta }_{L_{4:6}}\end{array}\right)$$

(57)

Numerical Simulations

To evaluate estimation quality, numerical simulations were carried out. In the simulations, we selected the north–east–down frame as the navigation frame. The attitude trajectory that lasted 200 s was generated by $\omega ={\left[-0.1cos\left(0.15t\right),0.1sin\left(0.1t\right),-0.1cos\left(0.05t\right)\right]}^{\mathrm{T}}\mathrm{rad}/\mathrm{s}$. The true trajectory without noise and bias is shown in Figure 2. Gyroscope bias was ${\mathit{b}}_{\omega }={\left[0.012,-0.021,0.014\right]}^{\mathrm{T}}\mathrm{rad}/\mathrm{s}$, and gyroscope noise was white Gaussian noise subjected to $\mathcal{N}\left(0,{\left({10}^{-3}\mathrm{rad}/\mathrm{s}\right)}^2\right)$. Two known corresponding measurement vectors in the navigation frame are ${\mathit{f}}_n={[0,0,g]}^{\mathrm{T}}$ and ${\mathit{m}}_n={[0.3197,0,0.4226]}^{\mathrm{T}}\mathrm{G}$, where $g=9.78\mathrm{m}/{\mathrm{s}}^2$ is the gravitational acceleration. The noises of the accelerometer and the magnetometer are white Gaussian noise and follow ${\mathit{v}}_f\sim \mathcal{N}\left(0,{\left(2\times {10}^{-3}g\right)}^2\right)$ and ${\mathit{v}}_m\sim \mathcal{N}\left(0,{\left(4\times {10}^{-3}\mathrm{G}\right)}^2\right)$, respectively. Some sensor parameters are provided by [42]. For filtering, we set the initial covariance matrix as ${\mathit{P}}_0=\mathrm{blkdiag}\left({10}^{-3}{\mathit{I}}_{3\times 3},{10}^{-7}{\mathit{I}}_{3\times 3}\right)$ and the initial gyroscope bias as ${\widehat{\mathit{b}}}_{\omega 0}={\left[0,0,0\right]}^{\mathrm{T}}\mathrm{rad}/\mathrm{s}$. The kernel scale and kernel length that constituted the kernel parameters were selected to be $0.001$ and $0.3$.

Three cases were considered in our simulations, and in the first two cases, we utilized the trajectory defined above but with different sets of initial attitudes. Case 1 randomly selected the initial attitude from uniform distribution between $-{10}^{\circ }$ and ${10}^{\circ }$, and Case 2 randomly chose the initial attitude from uniform distribution $\left[-{90}^{\circ },{90}^{\circ }\right]$. In Case 3, we used the angular velocity set above, but while 50 s $<t\le$ 60 s, angular velocity was set to be $\omega ={\left[-5,0.5,-0.5\right]}^{\mathrm{T}}\mathrm{rad}/\mathrm{s}$; while 100 s $<t\le$ 108 s, angular velocity was set to be $\omega ={\left[-0.1,0.1,-3\right]}^{\mathrm{T}}\mathrm{rad}/\mathrm{s}$; while 150 s $<t\le$ 152 s, angular velocity was set to be $\omega ={\left[0.1,-5,0.1\right]}^{\mathrm{T}}\mathrm{rad}/\mathrm{s}$. Initial attitudes of Case 3 were the same as those in Case 1.

For each case, we conducted 100 independent Monto Carlo simulations and utilized the following root-mean-square error (RMSE) and averaged RMSE of the Euler angles (roll, pitch, yaw) to evaluate estimation quality:

$${RMSE}_{att}\left(k\right)=\sqrt{\frac{1}{M}\sum _{m=1}^M{∥x_k^m-{\widehat{x}}_k^m∥}^2},$$

(58)

$${ARMSE}_{att}\left(k\right)=\sqrt{\frac{1}{MK}\sum _{k=1}^K\sum _{m=1}^M{∥x_k^m-{\widehat{x}}_k^m∥}^2},$$

(59)

where M is the total number of simulations; K is the simulation time step; $x_k^m$ and ${\widehat{x}}_k^m$ represent the truth Euler angles(simulated) and the estimated Euler angles, respectively.

With the simulated system and the initial conditions above, we compared nine different filters on Monte Carlo simulations:

*

$SO\left(3\right)$-based CKF ($SO\left(3\right)$ CKF) that considers the attitude embedded in a special orthogonal group ($SO\left(3\right)$) and the gyroscope bias in a vector space;

*

right invariant extended Kalman filter (Right-IEKF) and left invariant extended Kalman filter (Left-IEKF) in [21,23], where the gyroscope bias was treated as part of the Lie group structure;

*

right and left cubature Kalman filters on Lie groups (Right-CKF-LG and Left-CKF-LG) [36], which can be treated as extensions of UKF-LG in [16];

*

Bayes–Sard quadrature cubature Kalman filter ($SO\left(3\right)$ BSCKF) derived from [11] by utilizing the same Lie group action as the $SO\left(3\right)$ CKF;

*

our proposed right and left BSCKFs on Lie groups (Right-BSCKF-LG and Left-BSCKF-LG) in Section 4;

*

the proposed adaptive right BSCKF-LG with time-varying EMVs (Right-BSCKF-LG-adaptive).

We set the sliding window size in both the propagation and update steps of the Right-BSCKF-LG-adaptive to be $n=35$ in Cases 1 and 2, and $n=15$ in Case 3.

Simulation results of Case 1 are illustrated in Figure 3 and

Table 1. ARMSEs of Euler angles in Case 1.

Filters	ARMEs of Roll (deg)	ARMEs of Pitch (deg)	ARMEs of Yaw (deg)
$SO\left(3\right)$ CKF	$1.5172$	$1.5174$	$3.5564$
Left-CKF-LG	$0.5011$	$0.8032$	$2.2217$
Right-CKF-LG	$0.4571$	$0.7298$	$1.9835$
Left-IEKF	$0.5103$	$0.8185$	$2.1791$
Right-IEKF	$0.4463$	$0.7226$	$1.9114$
$SO\left(3\right)$ BSCKF	$0.2505$	$0.2351$	$0.7080$
Left-BSCKF-LG	$0.2156$	$0.1701$	$0.5407$
Right-BSCKF-LG	$0.2062$	$0.1708$	$0.5277$
Right-BSCKF-LG-adaptive	$0.2113$	$0.1745$	$0.4586$

. Figure 3 compares the RMSEs of the Euler angles, while Table 1 summarizes the corresponding ARMSEs. With small initial attitude errors, the proposed BSCKF-type filters with cubature points performed better than other filters did and needed less time to reach a steady state. Right-BSCKF-LG and its adaptive version obviously outperformed the other filters. Simulation results also verified that the performance of Right-IEKF and Right-CKF-LG was similar and slightly better than that of Left-IEKF and Left-CKF-LG under small initial estimation errors. The selection of sliding window size could affect the estimation accuracy of Right-BSCKF-LG-adaptive, and the filtering accuracy of Right-BSCKF-LG-adaptive was not significantly better than that of Right-BSCKF-LG.

Case 2 estimation results under large initial attitude errors are demonstrated in Figure 4 and

Table 2. ARMSEs of Euler angles in Case 2.

Filters	ARMEs of Roll (deg)	ARMEs of Pitch (deg)	ARMEs of Yaw (deg)
$SO$(3) CKF	$4.3881$	$3.2943$	$10.104$
Left-CKF-LG	$3.7219$	$2.6936$	$8.8371$
Right-CKF-LG	$1.5143$	$1.1443$	$4.1851$
Left-IEKF	$4.2617$	$2.9011$	$9.2021$
Right-IEKF	$0.7231$	$0.8164$	$2.4947$
$SO$(3) BSCKF	$1.9847$	$1.3373$	$5.6511$
Left-BSCKF-LG	$1.8803$	$1.2971$	$5.4425$
Right-BSCKF-LG	$0.6037$	$0.5515$	$1.8587$
Right-BSCKF-LG-adaptive	$0.5401$	$0.4604$	$1.4699$

. These graphs show that the estimation quality of $SO\left(3\right)$ CKF, Left-CKF-LG, and Left-IEKF was poor, especially that of $SO\left(3\right)$ CKF. In contrast, Right-CKF-LG and Right-IEKF achieved better accuracy and robustness than those of Left-CKF-LG and Left-IEKF. Again, BSCKF-type filters performed better. The estimation accuracy of Right-BSCKF-LG and Right-BSCKF-LG-adaptive was better than that of the other filters. Moreover, the accuracy of Right-BSCKF-LG-adaptive improved compared with that of Right-BSCKF-LG. Right-BSCKF-LG-adaptive performed best in the yaw angle and achieved the fastest convergence rate. As seen from the above analyses, the right estimation errors on Lie group $G=SO\left(3\right)\times {\mathbb{R}}^3$ are more suitable for attitude estimation with vector measurements.

In Case 3, we assumed that some fast motion occurred in the system trajectory. The true trajectory of Case 3 is shown in Figure 5. Estimation results under small initial attitude errors are indicated in Figure 6 and

Table 3. ARMSEs of Euler angles in Case 3.

Filters	ARMEs of Roll (deg)	ARMEs of Pitch (deg)	ARMEs of Yaw (deg)
$SO$(3) CKF	$18.5315$	$2.8001$	$15.7173$
Left-CKF-LG	$9.2549$	$1.4878$	$12.3105$
Right-CKF-LG	$15.2969$	$2.1041$	$13.0327$
Left-IEKF	$21.5772$	$2.6352$	$13.7434$
Right-IEKF	$21.6621$	$2.3658$	$15.1156$
$SO$(3) BSCKF	$14.3881$	$2.4143$	$17.4939$
Left-BSCKF-LG	$3.7793$	$1.2411$	$12.2423$
Right-BSCKF-LG	$8.4749$	$1.4365$	$10.6021$
Right-BSCKF-LG-adaptive	$3.7641$	$0.5966$	$2.4787$

. Figure 6 shows that the performance of all filters was worse compared to that in Case 1, partly because of the definition domains of Euler angles. However, the right and left BSCKF-LGs proposed in this paper were still outperformed among all filters, especially adaptive right BSCKF-LG. Table 3 summarizes the ARMSEs of Euler angles. Left BSCKF-LG was slightly more accurate than right BSCKF-LG in this case. The accuracy of Right-BSCKF-LG-adaptive was also greatly improved compared with that of Right-BSCKF-LG.

Figure 7 exhibits the computation times of the compared filters. CKF-LGs and BSCKF-LGs spent more computation time, while IEKFs spent minimal time. When the Bayes–Sard weights were calculated in advance according to the known system-state and measurement dimensions, the computation costs of BSCKF-LGs are almost equal to those of CKF-LGs. Right-BSCKF-LG-adaptive did not significantly increase the computational burden compared with Right-BSCKF-LG. Overall, BSCKF-LGs showed more accurate and robust estimation results in this attitude estimation example.

6. Conclusions

This article proposed a generalized Bayesian estimation algorithm on matrix Lie groups to calibrate numerical integration errors in classical sigma point filters on Lie groups using BSQMT theory. This Bayesian estimation is applicable to both measurements in Euclidean space and those evolving on the Lie group. Afterwards, with the proposed Lie group Bayesian estimator, we presented a Bayes–Sard cubature Kalman filter on Lie groups that comes in two variants. To obtain more accurate estimation, we then developed an approach to calculate adaptive EMVs. Numerical simulation results on quaternion attitude estimation indicated the superiority of our proposed filters over CKFs on Lie groups and unvariant EKFs. Future work includes exploring novel suitable methods to compute adaptive EMVs and applying the proposed filtering algorithm to visual–inertial navigation and fast drone navigation.