Dynamics on Lie groups with applications to attitude estimation

The impact of the Kalman filter (KF) in statistical estimation and control theory is difficult to overstate. The KF enables the calculation of the joint probability distribution of a vector of state variables $\mathbf{x}$ from a collection of noisy measurements supplemented with linear models of the state vector dynamics and measurement kalman1960new ; kalman1961new . The key assumptions required for the KF to be optimal – that dynamics and measurements are linear in the state space variables, that the noise appearing in dynamics and measurements is additive and white, and that the state vector distribution is initially Gaussian – are physically reasonable or can be made to hold approximately with good accuracy in many cases. This has led to the KF being used ubiquitously in guidance, navigation, and control grewal2007global , in robotics, in space and defense bar2004estimation ; ristic2004beyond , and myriad other applications. In places where the linearity assumption breaks down, the extended Kalman filter (EKF), in which the nonlinear models are linearized about the current state estimate point, or the unscented Kalman filter (UKF), in which a collection of nonlinear model outcomes are utilized to estimate state vector properties julier1997new , are widely employed, often to good accuracy. Some exact results are known for distributions other than Gaussians, though these usually require specific dynamical and measurement models to ensure the that resulting filter process leaves the form of the non-Gaussian distribution invariant and only updates its parameters; the work of Benes̆ benevs1981exact and Daum daum2005nonlinear is paradigmatic of this approach. At the extreme of nonlinear filtering is particle filters gordon1993novel , which represent the filtered distribution as a weighted collection of delta functions ristic2004beyond and so place no constraints on dynamical or measurement models or the underlying state vector distribution. However, particle filters are limited by a sampling error which can be slow to converge, especially for large state space dimension daum2003curse , and this can pose challenges for stability and efficient implementation.

A more subtle setting for nonlinear filtering is estimation of a state which is subject to a nonlinear constraint. Here, several complications can occur: linearization of the constraint may be inaccurate across the support of the state vector distribution and so key properties are not maintained. The constraint also reduces the effective dimensionality of the state vector, which introduces singularities into its associated covariance matrix that are known to cause issues for filtering. In such cases, a modification of the distribution is required to avoid non-physical predictions, but such modification will likely be at odds with the Gaussian assumptions required for the standard KF.

The present work investigates a special case of this situation in which the constraint is formulated as a requirement that the state vector lives on a Lie group, which is a mathematical space that is both a smooth manifold and a group. In this setting, the filtering problem may be translated from the Lie group to its associated tangent spaces (i.e., its Lie algebra), which enables the constraint to be maintained globally through the exponential map connecting the tangent spaces with the Lie group, allows for accurate linearization due to the "flat nature" of the tangent spaces, and enables an unconstrained, singularity-free representation of the state and covariance.

Before presenting our general theoretical results, we introduce the motivating application of attitude estimation from noisy, potentially biased gyro measurements in Sec. II. In addition to providing motivation for our research, this also allows for concrete example calculations alongside the theory. Our theoretical exposition starts with the most general setting in Section III, which is that of an arbitrary matrix Lie group $G$ with Lie algebra $\mathfrak{g}$ endowed with an evolution equation of the form $\dot{g}=f(g)$ for some vector field $f:G\rightarrow\mathfrak{g}$. The question of the necessary and sufficient conditions on $f$ for which the corresponding dynamics on $\mathfrak{g}$ are linear is then addressed. Leveraging the results of barrau2019linear , $f$ must satisfy the so-called group-affine property and we go beyond prior work to give a complete characterization of group-affine vector fields (see Theorem 1).

Section IV formally introduces the class of CGs on a matrix Lie group. The main result here is Corollary 1, which makes precise the claim that the class of CGs are invariant under the flow generated by a group-affine vector field. This Corollary provides the theoretical foundation for one of the main contentions in this work, namely that CGs are the most natural choice of probability distributions to filter with on a Lie group. Particular emphasis is then given to choosing a group law on the underlying state space which makes the state space dynamics in question group-affine. Section V considers an application-motivated stochastic generalization of group-affine dynamics and derives the corresponding SDE on $\mathfrak{g}$. This SDE on $\mathfrak{g}$ will be referred to as the tangent space SDE (TSSDE) on $\mathfrak{g}$ and is, in general, an SDE with state-dependent diffusion. This section ends with a discussion regarding the invariance of CGs in the presence of noise.

The theoretical results generated in Section III, IV, and V are used as motivation for our Lie group filtering methodology, detailed in Section VI. After introducing a discrete-time observation process, we design an (approximate) nonlinear filtering algorithm, called the TSF, that is based on the unscented transform (UT). Key novelties of this algorithm are the use of the continuous-time unscented transform (CTUT) sarkka2007unscented (generalized to state-dependent diffusion) to propagate moments through the TSSDE and a whitening procedure based on the Baker–Campbell–Hausdorff (BCH) formula. We also suggest how the TSF may be made more computationally efficient by considering a short-time expansion of the Fokker-Planck equation (FPE) associated with the TSSDE and deriving moment evolution equations based off this to replace to CTUT for moment propagation.

In Section VII, three application-relevant examples are worked out in detail. These three examples correspond to the well-known group $\mathrm{SU}_{2}$, the special Euclidean group $\mathrm{SE}_{3}$, and the group $\mathrm{SE}_{2,3}$, whose underlying state space is $\mathrm{SO}_{3}\times\mathbb{R}^{6}$ and is a simple generalization of $\mathrm{SE}_{3}$. The $\mathrm{SU}_{2}$ example is motivated by the problem of attitude filtering under the paradigm of dynamical model replacement (DMR) without gyro bias, while the $\mathrm{SE}_{2,3}$ example is motivated by the inertial navigation system (INS) state estimation problem without IMU biases. The group $\mathrm{SE}_{3}$ provides a bridge between these two examples. The natural dynamics for the $\mathrm{SU}_{2}$ problem falls into the class of constant Lie group dynamics, which are the simplest to analyze. On the other hand, the natural dynamics motivated by the INS problem are not constant, but fall into the more general class of group-affine dynamics. Emphasis is placed on how the $\mathrm{SE}_{2,3}$ group law on the underlying state space $\mathrm{SO}_{3}\times\mathbb{R}^{6}$ is the choice which results in group-affine dynamics (as compared to, e.g., the direct product (DP) group law). Additional insights regarding the fundamental solution of the FPE associated with the $\mathrm{SU}_{2}$ problem are provided at the end of Section VII.1.

A fourth example, covered in Section VIII, is the Lie group whose underlying manifold is $\mathrm{SO}_{3}\times\mathbb{R}^{3}$. This group is motivated by the problem of attitude estimation with the inclusion of the gyro bias that is to-be-estimated. This example will highlight the interplay between the equations of motion (i.e., dynamics), which are motivated by physics, and the freedom to choose a group structure which in turn makes the associated vector field group-affine. In this fourth example, we argue that there is no fixed group structure on $\mathrm{SO}_{3}\times\mathbb{R}^{3}$ with makes the attitude dynamics with gyro bias group-affine. Instead, a time-parameterized family of group laws must be considered to yield a vector field which is almost group-affine. This time-parameterized family of group laws is approximately $\mathrm{SE}_{3}$ to zeroth-order in time. (This observation is consistent with the proposal to use the $\mathrm{SE}_{3}$ group law for treating gyro bias in, e.g., barrau2022geometry .) This section ends with a numerical experiment that compares performance of the TSF using two different group laws on $\mathrm{SO}_{3}\times\mathbb{R}^{3}$, namely that induced by the DP and that of a semidirect product (i.e., $\mathrm{SE}_{3}$), to the Unscented Quaternion Estimator (USQUE) Crassidis_UKF . It is shown that the TSF using the $\mathrm{SE}_{3}$ group law exhibits near-perfect filter consistency, far outperforming the USQUE and TSF using the DP group law (Figure 1).

In attitude filtering, one is concerned with estimating an element of the non-abelian Lie group of $3$-dimensional spatial rotations, denoted as $\mathrm{SO}_{3}:=\{R\in\mathbb{R}^{3\times 3}:RR^{T}=I_{3},~{}\det{R}=1\}$. Specifically, a matrix $R\in\mathrm{SO}_{3}$ describes a rotation from one reference frame (e.g., the body frame of a rigid body) to another reference frame (e.g., an inertial or Earth-fixed frame). Since $\mathrm{SO}_{3}$ is a $3$-dimensional manifold, one may use, at least locally, a set of $3$ real parameters (e.g., Euler angles) to describe a matrix $R\in\mathrm{SO}_{3}$. However, since $\mathrm{SO}_{3}$ is not homeomorphic to $\mathbb{R}^{3}$, a single set of such parameters will necessarily have a singularity at some point (e.g., Euler angles have a singularity that cause the problem of gimbal lock).

The standard approach to avoid the issue of coordinate singularities is to work with a higher dimensional parameterization of rotations, such as the unit quaternions. A quaternion may be thought of as a vector $q\in\mathbb{R}^{4}$ which is partitioned as $q=(v~{}s)$, where $v=(v_{1}~{}v_{2}~{}v_{3})\in\mathbb{R}^{3}$ is the "vector" part and $s\in\mathbb{R}$ is the "scalar" part. A unit quaternion $q$ has $\|q\|=1$, where $\|\cdot\|$ indicates the standard Euclidean length in $4$ dimensions. The set of all unit quaternions is the $3$-sphere, denoted as $\mathbb{S}^{3}:=\{q\in\mathbb{R}^{4}:\|q\|=1\}$. Given a unit quaternion $q\in\mathbb{S}^{3}$, there is a map $R:\mathbb{S}^{3}\rightarrow\mathrm{SO}_{3}$ that associates the unit quaternion $q$ with a rotation $R(q)$ between two specified reference frames. Explicitly,

$\displaystyle R(q)=I_{3}-2s[v]_{\times}+2[v]_{\times}^{2},$

(1)

where $[v]_{\times}\in\mathbb{R}^{3\times 3}$ is the cross product matrix, which is the matrix satisfying $[v]_{\times}w=v\times w$ for all $w\in\mathbb{R}^{3}$. Note that $R(q)R^{T}(q)=I_{3}$ and $\det{R(q)}=1$ is a result of the constraint $\|q\|=1$.

One may endow $\mathbb{S}^{3}$ with a multiplication operation $\otimes$ such that the multiplication of two unit quaternions describes the composition of their individual rotations, i.e. $R(q\otimes q^{\prime})=R(q)R(q^{\prime})$. (Note that we are using M. Shuster’s convention for quaternion multiplication shuster1993survey . This choice is made so that the map $R$ in (1) is a (Lie) group homomorphism $\mathbb{S}^{3}\rightarrow\mathrm{SO}_{3}$. The reader should be aware that another convention is in use in the literature and which convention is being used should always be verified for consistency.) It may be shown that $q\otimes q^{\prime}$ defines another unit quaternion given by

$\displaystyle q\otimes q^{\prime}=\left(\begin{array}[]{c}sv^{\prime}+s^{\prime}v-v\times v^{\prime}\\ ss^{\prime}-\langle v,v^{\prime}\rangle\end{array}\right),$

(4)

where $\langle v,v^{\prime}\rangle=v_{1}v_{1}^{\prime}+v_{2}v_{2}^{\prime}+v_{3}v_{3}^{\prime}$ indicates the standard Euclidean inner product of the vectors $v,v^{\prime}\in\mathbb{R}^{3}$. This multiplication operation can be represented in matrix form as

$\displaystyle\left[q\otimes\right]:=sI_{4}+\mathcal{M}_{\mathfrak{s}}({v}),$

(5)

where $\mathcal{M}_{\mathfrak{s}}:\mathbb{R}^{3}\rightarrow\mathbb{R}^{4\times 4}$ is the anti-symmetric matrix

$\displaystyle\mathcal{M}_{\mathfrak{s}}({v})=\left(\begin{array}[]{cc}-[v]_{\times}&v\\[2.0pt] -v^{T}&0\end{array}\right),$

(8)

in which $v$ is interpreted as a column vector and $v^{T}$ as a row vector. Under the operation $\otimes$ the inverse quaternion is given by $q^{-1}=(-v~{}s)$ and the identity quaternion is given by $(0~{}1)$. Hence, $\mathbb{S}^{3}$ endowed with quaternionic multiplication forms a Lie group. As a Lie group, $\mathbb{S}^{3}$ is recognized as the double cover of $\mathrm{SO}_{3}$. The term double cover refers to the fact that the map (1) satisfies $R(q)=R(-q)$. In other words, there are two distinct unit quaternions that yield the same $\mathrm{SO}_{3}$-matrix $R$. Finally, it is worth noting that the Lie group $\mathbb{S}^{3}$ is typically identified with the special unitary group $\mathrm{SU}_{2}=\{U\in\mathbb{C}^{2\times 2}:UU^{\dagger}=I_{3},~{}\det{U}=1\}$.

If a rigid body is subject to an angular velocity $\omega(t)\in\mathbb{R}^{3}$ relative to a chosen reference frame and $R(t)\in\mathrm{SO}_{3}$ describes its orientation relative to that frame, then $\dot{R}=[\omega]_{\times}R$, where the "dot" denotes the time derivative. This equation of motion for rotations may be recast in terms of quaternions using the inverse of the map (1), which results in

$\displaystyle\left\{\begin{array}[]{l}\dot{q}=\dfrac{1}{2}\mathcal{M}_{\mathfrak{s}}({\omega})q\\[2.0pt] q(0)=q_{0}\in\mathbb{S}^{3},\end{array}\right.$

(11)

where $q_{0}$ describes the initial orientation of the rigid body. In the case of deterministic and constant $\omega(t)=\omega_{0}$, the solution is given by the matrix exponential:

$\displaystyle q(t)=e^{t\mathcal{M}_{\mathfrak{s}}({\omega_{0}})/2}q_{0}.$

(12)

Typically, the angular rate $\omega$ that is passed to (11) is a noisy measurement obtained from a gyroscope. A common parameterization of angular rate measurements is given by the Farrenkopf model farrenkopf1978analytic ; Crassidis_Extension . Stated as a system of coupled SDEs, the Farrenkopf model reads

$\displaystyle\left\{\begin{array}[]{l}\omega_{m}\mathrm{d}t=\left(\omega+\beta\right)\mathrm{d}t+\mathrm{d}\eta\\[2.0pt] \mathrm{d}\beta=\mathrm{d}\zeta,\end{array}\right.$

(15)

in which $\omega_{m}$ is the measured rate, $\omega$ is the true rate, $\beta$ is the rate bias, and $\mathrm{d}\eta$ and $\mathrm{d}\zeta$ are vectors of mutually uncorrelated Wiener processes whose derivatives are delta-correlated white noises with spectral density matrices $Q_{\eta}$ and $Q_{\zeta}$, respectively. This model excludes gyro misalignment and scale factor errors, but they can be straightforwardly included.

It must be stressed that the vector $\omega_{m}$ is a measurement, and so, strictly speaking, should be estimated when utilized in a filter Crassidis_Extension . Instead, the conventional approach, known as dynamic model replacement (DMR) Crassidis_UKF , is followed. DMR uses the measurements $\omega_{m}$ directly in the model and treats this measured angular rate as a constant over the propagation time step. The SDE describing the evolution of the quaternion under the Farrenkopf model is then determined by solving eqn. (15) for $\omega$ and plugging the result into eqn. (11). The result is the quaternion SDE:

$\displaystyle\left\{\begin{array}[]{l}\dot{q}=\dfrac{1}{2}\mathcal{M}_{\mathfrak{s}}({\omega_{m}-\beta-\eta})q\\[2.0pt] \dot{\beta}=\zeta\\[1.0pt] \|q\|=1.\end{array}\right.$

(19)

As an SDE, (19) must carry the Stratonovich interpretation, otherwise solutions may not maintain the constraint $\|q\|=1$. Eqn. (19) represents the SDE relevant for propagation in an attitude filter based on DMR using the Farrenkopf model (15).

A key question not yet addressed is: what is an appropriate family of probability distributions to describe the uncertainty associated with $q$? In this regard, let $\delta\sim\mathcal{N}^{3}(0,\Sigma)$ and $\mu\in\mathbb{S}^{3}$ be some fixed reference quaternion. Consider the distribution over $\mathbb{S}^{3}$ induced by $q=e^{\mathcal{M}_{\mathfrak{s}}({\delta})}\mu$ (c.f. (12)). The distribution associated with the random quaternion $q$ is an example of a CG on the Lie group $\mathbb{S}^{3}$ with parameters $\Sigma$ and $\mu$. With this family of distributions identified, the next question of immediate practical importance is: how do the parameters of the distribution evolve under (19)? Here, we find our first connection with the main results of our work. Namely, if $q_{0}=e^{\mathcal{M}_{\mathfrak{s}}({\delta_{0}})}\mu_{0}$ for some $\delta_{0}\sim\mathcal{N}^{3}(0,\Sigma_{0})$ and $\mu_{0}\in\mathbb{S}^{3}$, then the solution (12) is of the form $q(t)=e^{\mathcal{M}_{\mathfrak{s}}({\delta(t)})}\mu(t)$ for some $\delta(t)\sim\mathcal{N}^{3}(0,\Sigma(t))$ and $\mu(t)\in\mathbb{S}^{3}$. In other words, if $q_{0}$ is CG distributed, then $q(t)$ in (12) is also CG distributed (c.f. Corollary 1). The remainder of this section is concerned with explicitly demonstrating this claim. First, it is necessary to collect a few more facts about $\mathbb{S}^{3}$ and its Lie algebra.

Eqn. (5) gives us a convenient faithful representation $\rho$ of the group $\mathbb{S}^{3}$ as a subgroup of $\mathrm{SO}_{4}$, which is the Lie group of $4$ dimensional rotations, a higher dimensional generalization of $\mathrm{SO}_{3}$. (In what follows, we will write $\rho(\mathbb{S}^{3})$ to denote the subgroup of $\mathrm{SO}_{4}$ representing unit quaternions.) The convenience comes, in part, from the determination of the Lie algebra of $\rho(\mathbb{S}^{3})$, namely the set $\mathfrak{s}:=\{\mathcal{M}_{\mathfrak{s}}({a}):a\in\mathbb{R}^{3}\}\subset\mathfrak{so}_{4}$ where $\mathfrak{so}_{4}$ is the set of all $4\times 4$ antisymmetric matrices (i.e., the Lie algebra of $\mathrm{SO}_{4}$). That $\mathfrak{s}$ is the Lie algebra of $\rho(\mathbb{S}^{3})$ follows from the identity

$\displaystyle e^{\mathcal{M}_{\mathfrak{s}}({a})}=\cos{(\|a\|)}I_{4}+\frac{\sin{(\|a\|)}}{\|a\|}\mathcal{M}_{\mathfrak{s}}({a})\in\rho(\mathbb{S}^{3}),$

(20)

which follows from the Taylor series expansion of the matrix exponential and $\mathcal{M}_{\mathfrak{s}}({a})^{2}=-\|a\|^{2}I_{4}$.

Formula (20) is an instance of the exponential map $\exp:\mathfrak{g}\rightarrow G$ from the Lie algebra $\mathfrak{g}$ of a Lie group $G$, which is a concept that will be dealt with regularly in the sequel. The exponential map is defined for abstract Lie groups and their associated Lie algebras, but for the case of matrix Lie groups (i.e., closed subgroups of $\mathrm{GL}_{n}$), the abstract exponential map coincides with the standard matrix exponential. The Lie bracket on $\{\mathcal{M}_{\mathfrak{s}}({a}):a\in\mathbb{R}^{3}\}$ is given by

$\displaystyle[\mathcal{M}_{\mathfrak{s}}({a}),\mathcal{M}_{\mathfrak{s}}({b})]:=\mathcal{M}_{\mathfrak{s}}({a})\mathcal{M}_{\mathfrak{s}}({b})-\mathcal{M}_{\mathfrak{s}}({b})\mathcal{M}_{\mathfrak{s}}({a})=-2\mathcal{M}_{\mathfrak{s}}({a\times b}).$

(21)

In other words, the map $a\mapsto\mathcal{M}_{\mathfrak{s}}({a})$ is a Lie algebra isomorphism from the Lie algebra $(\mathbb{R}^{3},\times)$ to the matrix Lie algebra $\mathfrak{s}$. The physical interpretation of $e^{\mathcal{M}_{\mathfrak{s}}({a})/2}$ is that, when applied to a unit quaternion, it yields a rotation through the angle $\|a\|$ about the axis $a/\|a\|$.

Let $\omega_{0}\in\mathbb{R}^{3}$. Suppose $q(t)=e^{t\mathcal{M}_{\mathfrak{s}}({\omega_{0}})/2}q_{0}$ where $q_{0}=e^{\mathcal{M}_{\mathfrak{s}}({\delta_{0}})}\mu_{0}$ with $\delta_{0}\sim\mathcal{N}^{3}(0,\Sigma_{0})$ and $\mu_{0}\in\mathbb{S}^{3}$. The goal is to show that $q(t)$ is still of the form $q(t)=e^{\mathcal{M}_{\mathfrak{s}}({\delta(t)})}\mu(t)$ for some $\delta(t)\sim\mathcal{N}^{3}(0,\Sigma(t))$ and $\mu(t)\in\mathbb{S}^{3}$. The first step is to observe that

$\displaystyle q(t)=e^{t\mathcal{M}_{\mathfrak{s}}({\omega_{0}})/2}e^{\mathcal{M}_{\mathfrak{s}}({\delta_{0}})}\mu_{0}=e^{t\mathcal{M}_{\mathfrak{s}}({\omega_{0}})/2}e^{\mathcal{M}_{\mathfrak{s}}({\delta_{0}})}e^{-t\mathcal{M}_{\mathfrak{s}}({\omega_{0}})/2}\mu(t),$

where $\mu(t)=e^{t\mathcal{M}_{\mathfrak{s}}({\omega_{0}})/2}\mu_{0}$. What remains is to demonstrate that $e^{t\mathcal{M}_{\mathfrak{s}}({\omega_{0}})/2}e^{\mathcal{M}_{\mathfrak{s}}({\delta_{0}})}e^{-t\mathcal{M}_{\mathfrak{s}}({\omega_{0}})/2}=e^{\mathcal{M}_{\mathfrak{s}}({\delta(t)})}$ for some $\delta(t)\in\mathbb{R}^{3}$.

Recall a standard result in Lie theory, namely the braiding identity (hall2013lie, , Proposition 3.35):

$\displaystyle e^{X}Ye^{-X}=e^{\mathrm{ad}_{X}}Y,$

(22)

which holds for all $X,Y\in\mathfrak{g}$. In (22), $\mathrm{ad}_{X}:\mathfrak{g}\rightarrow\mathfrak{g}$ is given by $\mathrm{ad}_{X}Y:=[X,Y]$ and denotes the adjoint action on the Lie algebra $\mathfrak{g}$. Let $a,b\in\mathbb{R}^{3}$. The braiding identity (22), the commutator identity (21), and linearity of $\mathcal{M}_{\mathfrak{s}}$, yields

$\displaystyle e^{\mathcal{M}_{\mathfrak{s}}({a})}e^{\mathcal{M}_{\mathfrak{s}}({b})}e^{-\mathcal{M}_{\mathfrak{s}}({a})}=e^{e^{\mathcal{M}_{\mathfrak{s}}({a})}\mathcal{M}_{\mathfrak{s}}({b})e^{-\mathcal{M}_{\mathfrak{s}}({a})}}=e^{e^{\mathrm{ad}_{\mathcal{M}_{\mathfrak{s}}({a})}}\mathcal{M}_{\mathfrak{s}}({b})}=e^{\mathcal{M}_{\mathfrak{s}}({e^{-2[a]_{\times}}b})}.$

Applying the previous to identity to the expression $e^{t\mathcal{M}_{\mathfrak{s}}({\omega_{0}})/2}e^{\mathcal{M}_{\mathfrak{s}}({\delta_{0}})}e^{-t\mathcal{M}_{\mathfrak{s}}({\omega_{0}})/2}$, one finds that the desired $\delta(t)$ is given by $\delta(t)=e^{-t[\omega_{0}]_{\times}}\delta_{0}$. In other words, $\delta(t)\sim\mathcal{N}^{3}(0,\Sigma(t))$ where $\Sigma(t)=e^{-t[\omega_{0}]_{\times}}\Sigma_{0}e^{t[\omega_{0}]_{\times}}$. What has just been demonstrated is the invariance of the class of CGs on $\mathbb{S}^{3}$ under the flow generated by the dynamics (11). The deeper theory that is developed in Section III informs us that this invariance is really due to the fact that the vector field $q\mapsto\frac{1}{2}\mathcal{M}_{\mathfrak{s}}({\omega})q$ is group-affine on $\mathbb{S}^{3}$.

In general, additional uncertainty contributes to the attitude quaternion during propagation due to bias and noise corrupting the measured angular rates, as in (15). Some natural questions are then: are CGs invariant under propagation in the presence of bias and noise? If not, how should a CG approximation be maintained during propagation? These and related questions are addressed in generality throughout the remainder of this manuscript. In particular, in Section VI, the main result of Section V is used together with the CTUT to (approximately) propagate the parameters of a CG forward in time in the presence of noise. Once these tools are in hand, the problem of attitude filtering (with the inclusion of gyro bias) is revisited in Section VIII.

The main theoretical exposition begins with a determination on necessary and sufficient conditions for dynamics on a Lie group to result in linear dynamics on the associated Lie algebra. Let $G$ be a matrix Lie group of dimension $n$, and $\mathfrak{g}=\mathrm{Lie}({G})$ be its Lie algebra. Throughout this manuscript, $G$ is assumed to have surjective exponential map. Consider an initial value problem (IVP) on $G$ of the form

$\displaystyle\left\{\begin{array}[]{l}\dot{g}=f(g)\\ g(0)=g_{0}\in G,\end{array}\right.$

(25)

where $f:G\rightarrow\mathfrak{g}$ is a smooth map and the dot indicates time derivative. Here, $f$ is taken to be time-independent to avoid technical complications but this isn’t necessary. Suppose that the solution $g:[0,\infty)\rightarrow G$ to (25) satisfies $g(t)=e^{X(t)}\mu(t)$, where $X(t)$ has $X(0)=X_{0}\in\mathfrak{g}$, and $\mu(t)$ satisfies an ordinary differential equation (ODE) analogous to the first equation in (25):

$\displaystyle\left\{\begin{array}[]{l}\dot{\mu}=f(\mu)\\ \mu(0)=\mu_{0}\in G,\end{array}\right.$

with $\mu_{0}\in G$ known a priori. Our goal is to find the corresponding ODE for $X(t)\in\mathfrak{g}$, and then determine the conditions on $f$ that are required to make this ODE for $X$ affine (in the sense of affine maps on $\mathbb{R}^{n}$).

In order to derive an ODE for $X$, we recall the result in Lie theory known as the derivative of the exponential map (hall2013lie, , Theorem 5.4). Let $X:\mathbb{R}\rightarrow\mathfrak{g}$ be a $C^{1}$-path in the Lie algebra. The derivative of the exponential map is given by

$\displaystyle\frac{\mathrm{d}}{\mathrm{d}t}e^{X(t)}=e^{X(t)}J_{\mathfrak{g}}(X(t))\dot{X}(t),$

(26)

where $J_{\mathfrak{g}}(X)$ is referred to as the Jacobian of the exponential map and is given by

$\displaystyle J_{\mathfrak{g}}(X)=\int_{0}^{1}e^{-s\mathrm{ad}_{X}}\mathrm{d}s.$

(27)

(Note that some authors define the Jacobian with the $e^{X}$ on the right hand side (RHS) of (26) included.) Taking the time derivative of $g(t)=e^{X(t)}\mu(t)$, while using (26) and (25), results in $e^{X}J_{\mathfrak{g}}(X)\dot{X}\mu+e^{X}f(\mu)=f(e^{X}\mu)$, or, equivalently,

$\displaystyle\left\{\begin{array}[]{l}\dot{X}=J_{\mathfrak{g}}^{-1}(X)(e^{-X}f(e^{X}\mu)-f(\mu))\mu^{-1}\\[3.0pt] X(0)=X_{0}\in\mathfrak{g}.\end{array}\right.$

(30)

The goal now is to determine a condition on $f$ that makes the RHS of (30) a linear (affine) map of $X$. The relevant condition here is referred to as the group-affine property barrau2019linear . A vector field $f:G\rightarrow\mathfrak{g}$ is said to be group-affine if, for all $g_{1},g_{2}\in G$,

$\displaystyle f(g_{1}g_{2})=f(g_{1})g_{2}+g_{1}f(g_{2})-g_{1}f(I)g_{2},$

(31)

where $I\in G$ is the identity element. An important class of examples of group-affine vector fields are those of the form $f(g)=X_{1}g+gX_{2}$ for $X_{1},X_{2}\in\mathfrak{g}$. The ODE in (25) with $f(g)=X_{1}g$ is referred to as the (left) Poisson equation on the group $G$ in some of the literature (see, e.g., muller2021review ). However, in this manuscript, this special case is referred to as "constant" dynamics on the group, using an analogy with $\mathbb{R}^{n}$. Constant vector fields will be analyzed in detail in Section V. As shown in Theorem 1, not all group-affine vector fields are constant.

It is not entirely trivial to see how (31) forces (30) to simplify to a linear equation. Indeed, if $f$ is assumed to be group-affine and the identity (31) is applied to simplify the RHS of (30), then

$\displaystyle\left\{\begin{array}[]{l}\dot{X}=J_{\mathfrak{g}}^{-1}(X)(e^{-X}f(e^{X})-f(I))\\[3.0pt] X(0)=X_{0}\in\mathfrak{g}.\end{array}\right.$

(34)

Strikingly, while the dependence on $\mu$ was eliminated from the RHS of (30), eqn. (34) does not appear to reduce to a linear equation in $X$. Additional facts regarding group-affine vector fields beyond condition (31) are needed to argue that (34) is indeed a linear system.

Let $\psi_{t}:G\rightarrow G$ denote the flow associated with the equation $\dot{g}=f(g)$, where $f$ satisfies (31). The reason why (31) is called group-affine is owed to (barrau2019linear, , Proposition 8), which asserts that, for each $t>0$, there exists an element $\varphi_{t}\in\mathrm{Aut}({G})$ of the automorphism group $\mathrm{Aut}({G})$ of $G$ and $\nu_{t}\in G$ such that

$\displaystyle\psi_{t}(g_{0})=\varphi_{t}(g_{0})\nu_{t}.$

(35)

The terminology of "group-affine" now becomes clear by analogy with linear systems on $(\mathbb{R}^{n},+)$ since automorphisms of $(\mathbb{R}^{n},+)$ are precisely linear maps. Eqn. (35) is key to proving one implication in the following Theorem. Before presenting this Theorem, recall that a derivation is a linear map $D:\mathfrak{g}\rightarrow\mathfrak{g}$ that satisfies $D[X,Y]=[DX,Y]+[X,DY]$.

Proof. It is first argued that, if $f:G\rightarrow\mathfrak{g}$ is group-affine, then there exists a derivation $D:\mathfrak{g}\rightarrow\mathfrak{g}$ such that $f$ has the form (36). This argument will leverage three keys facts: 1) the flow associated with $f$ is of the form (35), 2) an automorphism of a Lie group induces an automorphism of the Lie algebra (40), and 3) the Lie algebra of $\mathrm{Aut}({\mathfrak{g}})$ (viewed as a Lie group) is equal to the set of derivations on $\mathfrak{g}$.

The starting point to show the RHS of (34) is a linear function of $X$ is (hall2013lie, , Theorem 5.6), which asserts the existence of a unique $T_{t}\in\mathrm{Aut}({\mathfrak{g}})$ such that

$\displaystyle\varphi_{t}(e^{X})=e^{T_{t}(X)},\hskip 14.22636pt\forall X\in\mathfrak{g},$

(40)

where $\varphi_{t}\in\mathrm{Aut}({G})$ is the automorphism appearing in (35). ($T_{t}\in\mathrm{Aut}({\mathfrak{g}})$ means that $T_{t}$ is an invertible linear map on $\mathfrak{g}$ that preserves brackets.) Using the automorphism property of $\varphi_{t}$,

$\displaystyle\psi_{t}(g_{0})=\psi_{t}(e^{X_{0}}\mu_{0})=\varphi_{t}(e^{X_{0}}\mu_{0})\nu_{t}=\varphi_{t}(e^{X_{0}})\varphi_{t}(\mu_{0})\nu_{t}=e^{T_{t}(X_{0})}\psi_{t}(\mu_{0}).$

By definition, $\psi_{t}$ satisfies $\frac{\mathrm{d}}{\mathrm{d}t}\psi_{t}(g_{0})=f(\psi_{t}(g_{0}))$ and $\lim_{t\rightarrow 0}\psi_{t}(g_{0})=g_{0}$. Therefore, by time-differentiating the identity $\psi_{t}(g_{0})=e^{T_{t}(X_{0})}\psi_{t}(\mu_{0})$ and using the definition of the flow, one finds that $T_{t}\in\mathrm{Aut}({\mathfrak{g}})$ must satisfy the ODE

$\displaystyle J_{\mathfrak{g}}(T_{t}(X_{0}))\dot{T}_{t}(X_{0})=e^{-T_{t}(X_{0})}f(e^{T_{t}(X_{0})})-f(I),$

(41)

which is precisely (34). Since $T_{t}\in\mathrm{Aut}({\mathfrak{g}})$, there exists a derivation $D:\mathfrak{g}\rightarrow\mathfrak{g}$ such that $T_{t}(X)=e^{tD}X$. This observation together with (41) implies that $f$ satisfies

$\displaystyle f(e^{X})=e^{X}J_{\mathfrak{g}}(X)DX+e^{X}f(I).$

(42)

For the reverse direction, suppose $f$ has the form (36) for some derivation $D:\mathfrak{g}\rightarrow\mathfrak{g}$. It is readily verified that (36) satisfies (31) when $D=0$. Therefore, without loss of generality, take $Y_{1}=Y_{2}=0$. Let $X_{1},X_{2}\in\mathfrak{g}$, and let $\mathrm{BCH}({X_{1}},{X_{2}})\in\mathfrak{g}$ be the element of $\mathfrak{g}$ such that

$\displaystyle e^{\mathrm{BCH}({X_{1}},{X_{2}})}=e^{X_{1}}e^{X_{2}}.$

(43)

In other words, $\mathrm{BCH}({X_{1}},{X_{2}})\equiv\log{e^{X_{1}}e^{X_{2}}}$. As is well-known and suggested by our notation, $\mathrm{BCH}({X_{1}},{X_{2}})\in\mathfrak{g}$ is the BCH series, which consists of a series of nested commutators of $X_{1}$ and $X_{2}$. With this context, the left hand side (LHS) of (31) reads

$\displaystyle f(e^{X_{1}}e^{X_{2}})=e^{X_{1}}e^{X_{2}}J_{\mathfrak{g}}(\mathrm{BCH}({X_{1}},{X_{2}}))D\hskip 1.42262pt\mathrm{BCH}({X_{1}},{X_{2}}),$

(44)

while the RHS of (31) reads

$\displaystyle e^{X_{1}}f(e^{X_{2}})+f(e^{X_{1}})e^{X_{2}}-e^{X_{1}}f(I)e^{X_{2}}=e^{X_{1}}e^{X_{2}}J_{\mathfrak{g}}(X_{2})DX_{2}+e^{X_{1}}J_{\mathfrak{g}}(X_{1})DX_{1}e^{X_{2}}.$

(45)

Hence, the objective is to argue that the following equality holds:

$\displaystyle e^{X_{1}}e^{X_{2}}J_{\mathfrak{g}}(\mathrm{BCH}({X_{1}},{X_{2}}))D\hskip 1.42262pt\mathrm{BCH}({X_{1}},{X_{2}})=e^{X_{1}}e^{X_{2}}J_{\mathfrak{g}}(X_{2})DX_{2}+e^{X_{1}}J_{\mathfrak{g}}(X_{1})DX_{1}e^{X_{2}}.$

Notice that $e^{\mathrm{BCH}({X_{1}},{X_{2}})}J_{\mathfrak{g}}(\mathrm{BCH}({X_{1}},{X_{2}}))D\hskip 1.42262pt\mathrm{BCH}({X_{1}},{X_{2}})=\frac{\mathrm{d}}{\mathrm{d}t}e^{e^{tD}\mathrm{BCH}({X_{1}},{X_{2}})}\Big{|}_{t=0}$. Since $D$ is a derivation, $e^{tD}\in\mathrm{Aut}({\mathfrak{g}})$. Using the fact that $\mathrm{BCH}({X_{1}},{X_{2}})$ is given by a series of nested commutators, $e^{tD}\mathrm{BCH}({X_{1}},{X_{2}})=\mathrm{BCH}({e^{tD}X_{1}},{e^{tD}X_{2}})$, and so

	$\displaystyle e^{\mathrm{BCH}({X_{1}},{X_{2}})}J_{\mathfrak{g}}(\mathrm{BCH}({X_{1}},{X_{2}}))D\hskip 1.42262pt\mathrm{BCH}({X_{1}},{X_{2}})$	$\displaystyle=\frac{\mathrm{d}}{\mathrm{d}t}e^{\mathrm{BCH}({e^{tD}X_{1}},{e^{tD}X_{2}})}\Big{|}_{t=0}$
		$\displaystyle=\frac{\mathrm{d}}{\mathrm{d}t}e^{e^{tD}X_{1}}e^{e^{tD}X_{2}}\Big{|}_{t=0}$
		$\displaystyle=e^{X_{1}}J_{\mathfrak{g}}(X_{1})DX_{1}e^{X_{2}}+e^{X_{1}}e^{X_{2}}J_{\mathfrak{g}}(X_{2})DX_{2}.$

The identity follows. $\square$

The definition of a CG on a (matrix) Lie group has been alluded to at several points in the preceding exposition. It is now appropriate to present a formal definition. First, consider a "matrization" map $\mathcal{M}_{\mathfrak{g}}:\mathbb{R}^{\dim{\mathfrak{g}}}\rightarrow\mathfrak{g}$ that associates an element of $\mathbb{R}^{\dim{\mathfrak{g}}}$ to its matrix representation in $\mathfrak{g}$ with respect to some basis for $\mathfrak{g}$ (c.f. eqn. (8)). Let $\xi\sim\mathcal{N}^{n}\left(0,\Sigma\right)$ be an $n$-dimensional Gaussian distributed vector with zero mean and covariance matrix $\Sigma$. Fix a group element $\mu\in G$, which is to be interpreted as the MAP estimate (this may also be interpreted as the Lie group mean wolfe2011bayesian ). From $\xi$ and $\mu$, induce a random element $g\in G$ via the formula

$\displaystyle g=e^{\mathcal{M}_{\mathfrak{g}}({\xi})}\mu.$

(46)

Write $g\sim\mathcal{T}_{G}(\mu,\Sigma)$ to indicate $g$ is a random element of $G$ with a distribution following (46). The distribution $\mathcal{T}_{G}(\mu,\Sigma)$ on $G$ induced via eqn. (46) is known as a (left) CG in the literature wolfe2011bayesian ; barrau2014intrinsic ; brossard2017unscented . A right CG has the $e^{\mathcal{M}_{\mathfrak{g}}({\xi})}$ and $\mu$ in (46) interchanged. A CG has the physically intuitive interpretation that there is a reference group element $\mu\in G$ representing the "best" estimate of the state and the uncertainty in the estimate is given by random “rotations" $e^{\mathcal{M}_{\mathfrak{g}}({\xi})}\in G$ generated by the random vector $\xi\in\mathbb{R}^{n}$. When the Lie group in question is $(\mathbb{R}^{n},+)$, then a CG is simply a Gaussian and the theory reduces to the standard Gaussian-based filtering framework on $\mathbb{R}^{n}$.

There is a decent volume of literature focused on proposing distributions on Lie groups for the purposes of filtering, and no attempt is made to provide a comprehensive review here (see, e.g., AttitudeSurvey07 ; barrau2014intrinsic ; WangLee2021 for relatively recent reviews and approaches for $\mathbb{S}^{3}$ and $\mathrm{SO}_{3}$). The class of CGs has been considered by other authors for the purposes of estimation on general Lie groups and, in particular, attitude estimation barfoot2014associating ; barrau2014intrinsic ; brossard2017unscented . However, one major contribution of this manuscript, in addition to Theorem 1, is the following corollary of Theorem 1.

Corollary 1 demonstrates that the class of CGs (46) is invariant under the flow generated by group-affine dynamics. The somewhat surprising aspect of this is that group-affine dynamics may be nonconstant (on the group), yet invariance is maintained (see, e.g., Section VII.3). For conventional Kalman filtering, it is well-known that linear state space dynamics take Gaussian initial conditions to Gaussians with updated parameters. What has just been proven is that for CGs the analogous condition on the vector field is that it is group-affine. This observation provides a case for the claim that CGs are the natural class of probability distributions on a Lie group to filter with respect to, assuming the dynamics under consideration are group-affine. (It is being assumed that the posterior distribution after an observation on the state is well approximated by a CG when making this claim. This may not always be the case.) In fact, given an underlying state space (i.e., a manifold of some dimension $\geq 1$) and a dynamical system on this state space, one is encouraged to design a group law which makes the associated vector field group-affine (c.f. barrau2022geometry ). If this is not possible, and one still insists on filtering with respect to CGs, then one should seek a group law which makes the dynamics "as close to group-affine" as possible. Numerical evidence for this claim is provided in the special case of attitude filtering in the presence of gyro bias (see Section VIII).

In this section, a stochastic generalization of a group-affine dynamical system as required for filtering is discussed. For simplicity, the focus is on the case where the deterministic part of the dynamics is constant (i.e., assuming $D=0$ in (36)). The more general case carries over straightforwardly with minimal modifications to the calculations below.

Suppose there is a smooth function $g:[0,\infty)\rightarrow G$ that satisfies

$\displaystyle\left\{\begin{array}[]{l}\dot{g}(t)=X(t)g(t)\\[3.0pt] g(0)=g_{0}\in G.\end{array}\right.$

(49)

for some (possibly time-dependent) $X(t)\in\mathfrak{g}$. The solution to (49) may be written in a Lie-algebraic way as $g(t)=e^{\Theta(t)}g_{0}$ with $\Theta(t)\in\mathfrak{g}$ satisfying $\left(\int_{0}^{1}e^{s\mathrm{ad}_{\Theta(t)}}\mathrm{d}s\right)\Theta^{\prime}(t)=X(t)$ and giving rise to the well-known Magnus expansion. In particular, if $[X(t_{1}),X(t_{2})]=0$ for all $t_{1},t_{2}\geq 0$, then $g(t)=e^{X(t)}g_{0}$.

Let $\eta:\mathbb{R}\rightarrow\mathbb{R}^{n}$ be a Brownian motion with delta-correlated spectral density matrix $Q_{\eta}$. Consider the following Langevin equation on $G$:

$\displaystyle\left\{\begin{array}[]{l}\dot{g}=(X+\mathcal{M}_{\mathfrak{g}}({\eta}))g\\[3.0pt] g(0)=g_{0}\in G.\end{array}\right.$

(52)

As an SDE, (52) must carry the Stratonovich interpretation, otherwise solutions may not remain in $G$. Suppose $g$ satisfying (52) is of the form $g(t)=e^{\mathcal{M}_{\mathfrak{g}}({\xi(t)})}\mu(t)$, where $\mu(t)\in G$ is given by $\mu(t)=e^{\Theta(t)}g_{0}$ and the equation for $\xi(t)\in\mathbb{R}^{n}$ is to-be-determined. In what follows, drop the implied $t$-dependence. Following a derivation similar to that of eqn. (30), $\dot{\xi}$ satisfies

$\displaystyle J_{\mathfrak{g}}(\mathcal{M}_{\mathfrak{g}}({\xi}))\mathcal{M}_{\mathfrak{g}}({\dot{\xi}})=e^{-\mathcal{M}_{\mathfrak{g}}({\xi})}(X+\mathcal{M}_{\mathfrak{g}}({\eta}))e^{\mathcal{M}_{\mathfrak{g}}({\xi})}-X.$

(53)

Applying the braiding identity (22) to (53) and simplifying yields

$\displaystyle J_{\mathfrak{g}}(\mathcal{M}_{\mathfrak{g}}({\xi}))\mathcal{M}_{\mathfrak{g}}({\dot{\xi}})=\left(e^{-\mathrm{ad}_{\mathcal{M}_{\mathfrak{g}}({\xi})}}-I\right)X+e^{-\mathrm{ad}_{\mathcal{M}_{\mathfrak{g}}({\xi})}}\mathcal{M}_{\mathfrak{g}}({\eta}).$

(54)

To simplify eqn. (54) further, introduce the operator $W_{\!\mathfrak{g}}:\mathfrak{g}\rightarrow\mathbb{R}^{n\times n}$ given by

$\displaystyle W_{\!\mathfrak{g}}(X)=\left(\int_{0}^{1}e^{s\mathrm{ad}_{X}}\mathrm{d}s\right)^{-1},$

(55)

and note $J_{\mathfrak{g}}^{-1}(X)e^{-\mathrm{ad}_{X}}=W_{\!\mathfrak{g}}(X)$. Noting further that $J_{\mathfrak{g}}^{-1}(X)(e^{-\mathrm{ad}_{X}}-I)=-\mathrm{ad}_{X}$, eqn. (54) simplifies to

$\displaystyle\mathcal{M}_{\mathfrak{g}}({\dot{\xi}})=\mathrm{ad}_{X}\mathcal{M}_{\mathfrak{g}}({\xi})+W_{\!\mathfrak{g}}(\mathcal{M}_{\mathfrak{g}}({\xi}))\mathcal{M}_{\mathfrak{g}}({\eta}).$

(56)