Nonlinear $H_{\infty}$ Filtering on the Special Orthogonal Group $SO(3)$ using Vector Directions

Rigid-body attitude estimation is a well-studied problem, and numerous solutions have been proposed [1, 2]. In general, attitude filtering problems are posed as optimization problems which seek to minimize a suitably defined cost function indicative of the filter’s performance. The cost function usually comprises a weighted average or integration of the estimation error, possibly with a terminal cost. For linear systems, most optimal filtering solutions are equivalent to Kalman filtering.

An alternative (pessimistic, or perhaps realistic) approach to attitude filtering is given by worst-case or nonlinear $H_{\infty}$ estimation. In formulating the $H_{\infty}$ filtering problem, one optimizes not for the average estimation error, but for the worst-case errors imparted by the worst-case disturbances and large initial estimation errors. The resulting filter respects a given upper bound on the energy gain from the disturbances and initial estimation errors to a chosen performance measure, also a function of the estimation error [3, 4, 5, 6, 7, 8].

Nonlinear $H_{\infty}$ filters have been applied successfully for spacecraft attitude determination using quaternions [2, 9, 10]. Since $H_{\infty}$ filters minimize only the worst-case error and not the mean square error (MSE), they are outperformed by the quaternion Multiplicative Extended Kalman Filter (MEKF), particularly in problem instances where the properties of the process and measurement noise/disturbance are perfectly known. However, for the more practical case of uncertain noise parameters leading to imperfect filter tuning, extensive Monte-Carlo simulations show that the MEKF gradually loses its edge, and that the $H_{\infty}$ filter performs better [11, 12]. Thus, nonlinear $H_{\infty}$ filtering offers a promising alternative to optimal estimation methods. Nevertheless, the quaternion MEKF remains the industry standard.

It is well-known that the attitude of a rigid body evolves on the set of rotation matrices, the Special Orthogonal Group $SO(3)$, and that all other attitude representations, including quaternions, suffer either from a singularity or a sign ambiguity [13]. Due to the globally defined and unique attitude representation provided by rotation matrices, the development of filtering solutions directly on the Lie group $SO(3)$ has received considerable attention in recent decades. The solutions proposed include nonlinear complementary filters [14, 15, 16, 17], symmetry-preserving observers [18, 19], the Invariant Extended Kalman Filter (IEKF) [20], and minimum energy filters [21, 22, 23, 24, 25]. Of these, the Geometric Approximate Minimum Energy (GAME) filter [24, 23], is especially relevant since it is an optimal estimation method that has been shown to significantly outperform the quaternion MEKF [26].

Given the potential benefits of worst-case filtering and the increasing interest in filtering algorithms on $SO(3)$, a promising solution might be to combine nonlinear $H_{\infty}$ filtering with attitude estimation using rotation matrices. This combination is the subject of the present work. We note that there has been some preliminary work in this direction. In particular, Lavoie et al. have proposed an invariant extended $H_{\infty}$ filter for Lie groups [27]. Noting the similarities and crucial differences between the Extended Kalman Filter (EKF) and its invariant counterpart, the IEKF, the authors consider an analogous relationship between the Extended $H_{\infty}$ Filter and its potential invariant counterpart. This simple and insightful observation leads them directly to an invariant extended $H_{\infty}$ filter for Lie groups. In contrast, we build on earlier results on nonlinear worst-case filtering using quaternions [9, 10], and extend them to the case of attitude estimation using rotation matrices. The main contribution of the paper is the development of a novel nonlinear $H_{\infty}$ filter on the Lie group $SO(3)$. It is envisaged that this exposition, which extends the preliminary results of [28], will pave the way for future developments, especially towards mixed $H_{2}/H_{\infty}$ filtering and possibly even the generalized $H_{\infty}$ regulator problem on $SO(3)$ yielding an estimator/controller pair which minimizes the impact of the worst-case disturbances on the controlled variables.

The rest of the discussion is organized as follows. After Section 1.1 introduces the notation used in the paper, Section 2 poses the $H_{\infty}$ filtering problem directly on $SO(3)$. Thereafter, the $H_{\infty}$ filter is derived in Section 3, and its structure is compared with that of the quaternion MEKF and the GAME filter. In Section 4, the proposed filter, the MEKF, and the GAME filter are simulated for different test conditions. Results indicate competitive performance. In particular, it is seen that the GAME filter has the best transient performance, whereas the $H_{\infty}$ filter demonstrates better convergence than the MEKF. At steady-state, the three filters have similar performance. Finally, in Section 5, the main conclusions of the study are reviewed.

This section formulates the attitude estimation problem using rotation matrices. In particular, we assume the following kinematic model:

where the state $R(t)$ evolves on $SO(3)$, and represents the orientation of a body-fixed frame with respect to an inertial reference frame. In the kinematics equation, the vectors $\omega,\delta\in\mathbb{R}^{3}$ are body-frame vectors denoting the measured angular velocity and the process disturbance, respectively. In the output equation, the vectors $\left\{r_{i}\right\}$ specify known vector directions in the reference frame, and the vectors $\left\{y_{i}\right\}$ represent their measurements in the body-fixed frame with corresponding measurement errors $\left\{\varepsilon_{i}\right\}$. Lastly, the coefficients $g$ and $\left\{k_{i}\right\}$ scale the process disturbance and measurement errors, respectively, and represent tuning gains for the filter proposed in Section 3.

We consider the following estimator:

where the filter gain $G_{q}\left(\hat{R},t\right)$ is a $3$-by-$3p$ matrix which will be specified later, and

represents the estimate for the $i$-th reference direction $r_{i}$. We note that this estimator structure is similar to the one considered in [9] for deriving a quaternion-based $H_{\infty}$ filter.

For the augmented system (3)-(4), the estimation error is defined as

Using the axis-angle representation, the estimation error can be expressed as

where $e\in\mathbb{S}^{2}$ denotes the axis of rotation between the body-fixed and estimate frames, and $\tilde{\theta}$ denotes the angle of rotation. We consider the following configuration error vector as a penalty variable or generalized estimation error:

Using the axis-angle representation, the penalty variable can be expressed as

We recall that the optimal $H_{\infty}$ filtering problem, for the augmented system (3)-(4), seeks a filter gain which minimizes the $\mathcal{L}_{2}$ gain from the process disturbance $\delta$, the measurement errors $\left\{\varepsilon_{i}\right\}$, and the initial conditions $R_{0}=R\left(0\right)$, $\hat{R}_{0}=\hat{R}\left(0\right)$ to the penalty variable $z\left(t\right)$. Since this is, in general, a difficult problem to solve, we resort to the sub-optimal $H_{\infty}$ filtering problem which ensures that the $\mathcal{L}_{2}$ gain respects a given upper bound $\gamma$, see e.g., [4, 5, 6, 9]. More precisely, we seek a filter gain such that for all $t\geq 0$, $\delta$, $\left\{\varepsilon_{i}\right\}$ , $R_{0}$, and $\hat{R}_{0}$, the following dissipation inequality is satisfied:

As has been well established in the literature on nonlinear $H_{\infty}$ control, see e.g., [3, 5, 6, 9, 10], the requirement (10) on the $\mathcal{L}_{2}$ gain is closely related to the notion of dissipativity. In particular, we seek a filter gain $G_{q}(\hat{R},t)$ and a smooth positive semidefinite storage function $V(R,\hat{R},t)$ such that the following dissipation inequality is satisfied:

Next, we obtain an expression for the time derivative of the penalty variable $z\left(t\right)$. In particular, from (8), we have that

Conseqently, we can express the time derivative of the penalty variable as

Substituting the dynamics from (3) and (4), we can express the time derivative of $\tilde{R}$ as follows:

Consequently, we have that:

We apply the following identity to each term on the right-hand side of the above expression:

For the first two terms, we observe that:

In the above simplification, the last equality follows from the relation (12). Next, we introduce the following mapping:

Consequently, we can express (13) as:

Removing the $\times$ operator, and substituting for $\tilde{y}$ and $\hat{y}_{i}$ from (4)-(5), we obtain:

This section presents a novel $H_{\infty}$ filter for attitude estimation using rotation matrices. The filter derivation follows the same procedure as is used in [9, 10] for deriving a quaternion-based $H_{\infty}$ filter. In particular, starting from a storage function on $SO\left(3\right)$, a dissipation inequality of the form (11) is considered. The worst-case process disturbance and measurement errors are obtained, as well as the gain which optimizes for the worst-case disturbances. These values are used to simplify the dissipation inequality. Thereafter, for sufficiently small estimation errors, a deterministic nonlinear $H_{\infty}$ filter is derived, the filter gain $G_{q}$ is specified, and a Riccati-type gain update equation is obtained.

We consider the following storage function:

where the penalty variable $z(t)$ is defined as in (8), and $P(t)$ is a symmetric, positive definite, $3$-by-$3$ matrix. The time derivative of the storage function is given by

Substituting (15) in (17), the dissipation inequality (11) can be expressed as: