Stochastic H_∞ Filtering of the Attitude Quaternion ^†

For simplicity and clarity, drift-free gyroscopes are treated first. Consider the following Itô stochastic differential equation (SDE) for the attitude quaternion, and the associated measurement equation [7]: where ${\mathbf{q}}_t\in {\mathbb{R}}^4$ denotes the attitude quaternion, ${\mathsf{\Omega }}_t\in {\mathbb{R}}^{4\times 4}$ is the following matrix function of the measured angular velocity ${\omega }_t$, where ${\omega }_t\in {\mathbb{R}}^3$, which is acquired by a triad of body-mounted gyroscopes, is corrupted by an additive standard Brownian motion, ${\beta }_t\in {\mathbb{R}}^3$, with infinitesimal independent increments ${\mathbf{d}\beta }_t$, such that $E\left\{{\mathbf{d}\beta }_t{\mathbf{d}\beta }_t^T\right\}=I_{3}dt$. The parameter ${\sigma }_{ϵ}$ denotes the standard deviation of the gyro noise ${\beta }_t$. The drift term in Equation (1) features a damping term in ${\sigma }_{ϵ}$ that ensures mean-square stability of the process ${\mathbf{q}}_t$ (see [7,8]). Under the conditions governing Equation (1), it was shown in [7] that the trace of $E\left\{{\mathbf{q}}_t{\mathbf{q}}_t\right\}$ is invariant as t increases, and in [8] that the steady-state of $E\left\{{\mathbf{q}}_t{\mathbf{q}}_t\right\}$ as t tends to infinity is the scaled identity matrix $\frac{1}{4}{I}_4$. Equation (2) is a continuous-time quaternion measurement equation where the measurement value is identically zero [6]. Hence, and the measurement matrix $H_t\in {\mathbb{R}}^{4\times 4}$ is constructed from LOS measurements. Let ${\mathbf{b}}_t\in {\mathbb{R}}^3$ and ${\mathbf{r}}_t\in {\mathbb{R}}^3$ denote the projections of a measured LOS in the spacecraft body frame axes and a reference inertial frame, respectively, then $H_t$ is computed as follows The matrix, $\mathsf{\Xi }\left({\mathbf{q}}_t\right)$, which appears both in the process and measurement multiplicative noises, is the following linear matrix function of the quaternion ${\mathbf{q}}_t={\left[{\mathbf{e}}_t^T{q}_t\right]}^T$: The measurement noise is modeled as a standard Brownian motion, ${\eta }_t\in {\mathbb{R}}^3$, multiplied by the parameter ${\sigma }_b$. Assume that ${\sigma }_{ϵ}$ and ${\sigma }_b$ are unknown, possibly time-varying random parameters, and that there is no prior knowledge of their possible values or bounds. The filtering problem consists of estimating the quaternion ${\mathbf{q}}_t$ in the presence of unknown and random ${\sigma }_{ϵ}$ and ${\sigma }_b$ and is formulated as a disturbance attenuation problem. The following estimator is considered: The Itô correction term, $\frac{3{\sigma }_{ϵ}^2}{4}$, is dropped for simplicity. It is not required for the filter development as the dynamics of the filter estimate and estimation error, $\{{\widehat{\mathbf{q}}}_t,{\tilde{\mathbf{q}}}_t\}$, are designed to be stable in the mean-square sense independently from that correction [16]. Let ${\tilde{\mathbf{q}}}_t$ denote the additive quaternion estimation error, i.e., Given a scalar $\gamma >0$, a gain process $K\left({\widehat{\mathbf{q}}}_t\right)\in {\mathbb{R}}^{4\times 4}$ is sought, such that the following $H_{\infty }$ criterion is satisfied: under the constraints (1) and (2), and where ${\mathbf{v}}_t$ denotes the augmented process of admissible disturbance functions. Whenever Equation (12) is true, the so-called $L_2$-gain property from $\{{\tilde{\mathbf{q}}}_0,{\mathbf{v}}_t\}$ to ${\tilde{\mathbf{q}}}_t$, is satisfied for $0\le t\le T$. Two cases for $\mathbf{v}$ will be considered in the following: $\mathbf{v}={\sigma }_{ϵ}$ and $\mathbf{v}=\{{\sigma }_{ϵ},{\sigma }_b\}$ and significant differences will be highlighted.

Following standard techniques, the following augmented process is defined: The design model for the process ${\mathbf{q}}_t$ obeys the following equation The estimator’s equation is given by Equation (9), i.e., where ${\widehat{K}}_t$ denotes $K\left({\widehat{\mathbf{q}}}_t\right)$. Using Equations (2), (14) and (15) yields the SDE for the estimation error: where $\mathsf{\Xi }$ denotes the matrix $\mathsf{\Xi }\left(\mathbf{q}\right)$, ${\mathsf{\Xi }}_{Ci}$, $i=1,2,3$, denote the columns of the matrix $\mathsf{\Xi }$, and the scalar processes ${\beta }_i$, and $i=1,2,3$ are the components of the vector Brownian motion ${\beta }_t$. Notice that $\mathsf{\Xi }$ is a function of the augmented process $\{{\widehat{\mathbf{q}}}_t,{\tilde{\mathbf{q}}}_t\}$, as it is a function of the quaternion $\mathbf{q}$. Appending Equations (15) and (16) yields the following augmented SDE: where $O_{m\times n}$ denotes an $m\times n$ matrix of zeros. Equation (17) can be re-written in the following compact form: where $F^a$, ${\mathbf{g}}_2^i\left({\mathbf{q}}_t^a\right)$, and $G\left({\mathbf{q}}_t^a\right)$ are effectively defined from Equation (17). Equation (17) provides a process equation for the augmented state ${\mathbf{q}}_t^a$ with Brownian motions as the inputs, state-dependent input gain matrices, and here a state-linear drift term.

The desired $L_2$-gain property will be satisfied if and only if the augmented system (18) is dissipative with respect to the supply rate $S(\mathbf{v}\left(t\right),{\mathbf{q}}_t^a)={\gamma }^2{\parallel \mathbf{v}\left(t\right)\parallel }^2-{\parallel {\tilde{\mathbf{q}}}_t\parallel }^2$, for a given positive scalar $\gamma$ [16]. Thus, a non-negative scalar-valued function, $V({\mathbf{q}}^a,t)$, is sought that satisfies the fundamental property [17]: for all ${\mathbf{q}}^a$ and for all admissible $\mathbf{v}\left(t\right)$. When Equation (19) is satisfied, the function V is called a storage function for the supply rate S. A sufficient condition for Equation (19) is as follows: for all ${\mathbf{q}}^a$ and for all admissible $\mathbf{v}\left(t\right)$, where $dV$ is the Itô differential of the function V. Notice that the processes $\left\{{\mathsf{\Omega }}_t\right\}$ and $\left\{H_t\right\}$ are observed and thus belong to the information pattern. Let ${\mathcal{I}}^t$ denote the information pattern at time t, which includes the observations of the angular velocity and of the LOS until t, i.e., ${\mathcal{I}}^t=\{{\left\{\omega \right\}}_0^t,{\left\{\mathbf{b}\right\}}_0^t\}$. In the development of the sufficiency conditions, one can use the following property of the expectation operator to write (21) as follows: for all ${\mathbf{q}}^a$ and t. Then, a sufficient condition for (22) is for all ${\mathbf{q}}^a$ and t. Given the conditioning on ${\mathcal{I}}^t$ in (23), the functions ${\mathsf{\Omega }}_t$ and $H_t$ can be considered deterministic functions. Using the Itô differentiation rule ([19], p. 112) and dropping the expectation operator on both sides of Equation (21) yields the following sufficient condition, i.e., the Hamilton–Jacobi–Bellman (HJB) inequality for $V({\mathbf{q}}^a,t)$: for all $0\le t\le T$, ${\mathbf{q}}^a$, and for all admissible $\mathbf{v}\left(t\right)$, where G and ${\mathbf{g}}_2^i$ are functions of ${\mathbf{q}}^a$. Dropping the integral and the expectation operators is a standard procedure in stochastic $H_{\infty }$ estimation and control [16] or in stochastic optimal control ([20], p. 321).

A standard approach to circumvent the formidable task of solving the partial differential inequality for V [Equation (24)] consists of guessing the solution in a parameterized form and developing sufficient conditions for its parameters. The classical quadratic form for V will be used here: where $P_t$ is assumed to be symmetric, positive, and block diagonal, i.e.,

Using Equations (36) and (37) in Equation (31), straightforward computations yield the following identity: where ${\widehat{F}}_t$ is defined as follows: Exploiting the following property of the matrix $\mathsf{\Xi }$ ([9], Eq. (8.20b)), yields the following identities

Given ${\widehat{\mathbf{q}}}_0$, choose $\tilde{P}\left(0\right)$, such that Equation (20) is satisfied. Solve the following set of (differential) LMIs for ${\tilde{P}}_t={\tilde{P}}_t^T>0\in {\mathbb{R}}^4$, ${\tilde{Y}}_t\in {\mathbb{R}}^4$, and $W_t=W_t^T>0\in {\mathbb{R}}^4$:

Assuming that the rate gyro error consists of white noise and a bias, we consider the following stochastic dynamical system in Itô form: where ${\mathbf{c}}_t$ denotes the additive drift, modeled as a random walk process with mean ${\mathbf{c}}_0$ and variance parameter ${\sigma }_c\left(t\right)$. In Equation (77), ${\nu }_t$ denotes a standard Brownian motion that is independent of ${\beta }_t$ and ${\eta }_t$. Equations (76) and (77) stem from a straightforward extension of the quaternion SDE of Section 2.

The filtering problem consists of estimating the quaternion ${\mathbf{q}}_t$ and the gyro drift ${\mathbf{c}}_t$ from the LOS measurements in the presence of unknown noise standard deviations ${\sigma }_{ϵ}\left(t\right)$, ${\sigma }_b\left(t\right)$, and ${\sigma }_c\left(t\right)$. Assuming that ${\sigma }_{ϵ}\left(t\right)$, ${\sigma }_b\left(t\right)$, and ${\sigma }_c\left(t\right)$ are stochastic non-anticipative processes with finite second-order moments, we consider the following estimator: Let ${\tilde{\mathbf{q}}}_t$ and ${\tilde{\mathbf{c}}}_t$ denote the additive quaternion and biases estimation error, i.e., Given a scalar $\gamma >0$, we seek the gains $K_q,K_c$, such that the following $H_{\infty }$ criterion is satisfied: under the constraints (76)–(78), and where ${\mathbf{v}}_t$ denotes the augmented process of admissible disturbance functions, i.e., ${\mathbf{v}}_t=\{{\sigma }_{ϵ}\left(t\right),{\sigma }_b\left(t\right),{\sigma }_c\left(t\right)\}$. Whenever Equation (84) is true, it is said that the $L_2$-gain property is satisfied from $\{{\tilde{\mathbf{q}}}_0,{\tilde{\mathbf{c}}}_0,{\mathbf{v}}_t\}$ to $\{{\tilde{\mathbf{q}}}_t,{\tilde{\mathbf{c}}}_t\}$, for $0\le t\le T$.

The SDE of the quaternion-drift system is compactly rewritten as follows: and the estimator is rewritten as follows: The augmented process $\{{\widehat{\mathbf{q}}}_t,{\widehat{\mathbf{c}}}_t,\tilde{\mathbf{q}},\tilde{\mathbf{c}}\}$ is governed by the following SDE where second-order terms with respect to the noises ${\beta }_t$, ${\nu }_t$, and ${\eta }_t$ and to the estimation errors $\tilde{\mathbf{q}}$, $\tilde{\mathbf{c}}$ have been neglected. Equation (87) may be re-written in the following compact form: The remainder of the filter development is straightforward and is omitted for the sake of brevity.