# Nonlinear Stochastic Attitude Filters on the

Special Orthogonal Group 3: Ito and Stratonovich

###### Abstract

This paper formulates the attitude filtering problem as a nonlinear stochastic filter problem evolved directly on the Special Orthogonal Group 3 (). One of the traditional potential functions for nonlinear deterministic complimentary filters is studied and examined against angular velocity measurements corrupted with noise. This work demonstrates that the careful selection of the attitude potential function allows to attenuate the noise associated with the angular velocity measurements and results into superior convergence properties of estimator and correction factor. The problem is formulated as a stochastic problem through mapping to Rodriguez vector parameterization. Two nonlinear stochastic complimentary filters are developed on . The first stochastic filter is driven in the sense of Ito and the second one considers Stratonovich. The two proposed filters guarantee that errors in the Rodriguez vector and estimates are semi-globally uniformly ultimately bounded in mean square, and they converge to a small neighborhood of the origin. Simulation results are presented to illustrate the effectiveness of the proposed filters considering high level of uncertainties in angular velocity as well as body-frame vector measurements.

To cite this article:
H. A. Hashim, L. J. Brown, and K. McIsaac, ”Nonlinear Stochastic Attitude Filters on the Special Orthogonal Group 3: Ito and Stratonovich,” IEEE Transactions on Systems, Man, and Cybernetics: Systems, vol. PP, no. PP, pp. 1–13, 2018.

The published version (DOI) can be found at: 10.1109/TSMC.2018.2870290

Please note that where the full-text provided is the Author Accepted Manuscript or Post-Print version this may differ from the final Published version. To cite this publication, please use the final published version.

Personal use of this material is permitted. Permission from the author(s) and/or copyright holder(s), must be obtained for all other uses, in any current or future media, including reprinting or republishing this material for advertising or promotional purposes.

Publication information:

Date of Submission: January 2018.

Date of Acceptance: September 2018.

Available Online: October 2018.

Please contact us and provide details if you believe this document breaches copyrights. We will remove access to the work immediately and investigate your claim.

## I Introduction

This paper concerns the problem of attitude estimation of a rigid-body rotating in 3D space. In fact, attitude estimation is one of the major sub-tasks in the field of robotics. The attitude can be constructed from a set of vector measurements made on body-frame and reference-frame as it acts as a linear transformation of one frame to the other. Generally, the attitude estimation problem aims to minimize the cost function such as Wahba’s Problem [1]. The earliest work in [1] was purely algebraic. Several alternative methods attempted to reconstruct the attitude simply and statically by solving a set of simultaneous known inertial and body-frame measurements, for instance, TRIAD or QUEST algorithms [2, 3] and singular value decomposition (SVD) [4]. However, vectorial measurements are subject to significant noise and bias components. Therefore, the category of static estimation in [2, 3, 4] gives poor results in this case. Consequently, the attitude estimation problem can be tackled either by Gaussian filter or nonlinear deterministic filter.

In the last few decades, several Gaussian filters have been developed mainly to obtain higher estimation performance with noise reduction. Many attitude estimation algorithms are based on optimal stochastic filtering for linear systems known as Kalman filter (KF) [5]. The linearized version of KF can be modified in a certain way for nonlinear systems to obtain the extended Kalman filter (EKF) [6]. An early survey of attitude observers was presented in [7] and a more recent overview on attitude estimation was introduced in [8]. Over the last three decades, several nonlinear filters have been proposed to estimate the attitude of spacecrafts. However, EKF and especially the multiplicative extended Kalman filter (MEKF) is highly recommended and considered as a standard in most spacecraft applications [8]. Generally, the covariance of any noise components introduced in angular velocity measurements is taken into account during filter design. The family of KFs parameterize the global attitude problem using unit-quaternion. The unit-quaternion provides a nonsingular attitude parameterization of attitude matrix [9]. Also, the unit-quaternion kinematics and measurement models of the attitude can be defined by a linear set of equations dependent on the quaternion state through EKF. This advantage motivated researchers to employ the unit-quaternion in attitude representation (for example [7, 10]). Although EKF is subject to theoretical and practical problems, the estimated state vector with the approximated covariance matrix gives a reasonable estimate of uncertainties in the dynamics. In general, a four-dimensional vector is used to describe a three-dimensional one. Since, the covariance matrix associated with the quaternion vector is , whereas the noise vector is , the covariance is assumed to have rank 3. Generally, the state vector is as it includes the four quaternion elements and the three bias components. One of the earliest detailed derivations of EKF attitude design was presented in [7]. However, the unit-quaternion kinematics and measurement models can be modified to suit KF with a linear set of equations [11]. The KF in [11] has the same state dimensions as EKF and to some degree, it can outperform the EKF. MEKF [10] is the modified version of EKF and it is highly recommended for spacecraft applications [8]. In MEKF, the true attitude state is the product of reference and estimated error quaternion. The estimated error in quaternion is parameterized from a three-dimensional vector in the body-frame, and the error is estimated using EKF. Next, the MEKF is used to multiply the estimated error and the reference quaternion. The estimated error should be selected in such a way that it yields identity when multiplied by the reference quaternion. The EKF can be modified into invariant extended Kalman filter (IEKF), which has two groups of operations. The right IEKF considers the errors modeled in the inertial-frame and the left IEKF matches with the MEKF [12]. IEKF has autonomous error and its evolution error does not depend on the system trajectory. A recently proposed attitude filtering solution known as geometric approximate minimum-energy filter (GAMEF) approach [13] is based on Mortensen’s deterministic minimum-energy [14]. Unlike KF, EKF, IEKF, and MEKF, the GAMEF kinematics are driven directly on . In addition, KF, EKF, and IEKF are based on first order optimal minimum-energy which makes them simpler in computation and implementation. In contrast, MEKF and GAMEF are second order optimal minimum-energy, and therefore they require more calculation steps and more computational power. The Unscented Kalman filter (UKF) uses the unit-quaternion kinematics, and its structure is nearly similar to KF, however, UKF utilizes a set of sigma points to enhance the probability distribution [15]. In spite of the fact that UKF requires less theoretical knowledge and outperforms EKF in simulations, it requires more computational power, while the sigma points could add complexity to the implementation process [16]. Particle filters (PFs) belong to the family of stochastic filters, but they do not follow the Gaussian assumptions [17]. The main idea of PFs is the use of Monte-Carlo simulations for the weighted particle approximation of the nonlinear distribution. In fact, PFs outperform EKF, however, they have higher computational cost, and they do not fit small scale systems [8]. Moreover, they do not have a clear measure of how close the solution is to the optimal one [13]. Quaternion based attitude PF showed a better performance than UKF with higher processing calculations [18]. All the Gaussian filters described above as well as PFs are based on unit-quaternion, where the main advantage is non-singularity in attitude parameterization, while the main drawback is non-uniqueness in representation.

Aside from Gaussian filtering methods, nonlinear deterministic filters provide an alternative solution of attitude estimation which aims to establish convergence bounds with stable performance. Indeed, inertial measurement units (IMUs) have a prominent role in enriching the research of attitude estimation [19, 20, 21]. IMUs fostered researchers to propose nonlinear deterministic complementary filters on using vectorial measurements with the need of attitude reconstruction [19, 22] or directly from vectorial measurements without attitude reconstruction [19, 23]. Also, the work done in [19] provides the filter kinematics in quaternion representation. In general, nonlinear deterministic filters achieve almost global asymptotic stability as they disregard the noise impact in filter derivation.

Nonlinear deterministic attitude filters have three distinctive advantages, such as better tracking performance, less computational power, and simplicity in derivation when compared to Gaussian filters or PFs [8]. Furthermore, no sensor knowledge is required in nonlinear deterministic filters, due to the fact that they omit the noise component in filter derivation. Overall, nonlinear deterministic attitude filters outperform Gaussian filters [19]. Observers play a crucial role in different control applications, especially for nonlinear stochastic systems (for example [24, 25, 26]). Aside from attitude observers, control applications are utilized for nonlinear systems with uncertain components [27, 28]. These applications could include robust stabilization [29], control of uncertain nonlinear multi-agent systems [30, 31], and stochastic nonlinear control for time-delay systems [32].

Two major challenges have to be taken into account when designing the attitude estimator, 1) the attitude problem of the rigid-body, modeled on the Lie group of , is naturally nonlinear; and 2) the true attitude kinematics rely on angular velocity. Therefore, successful attitude estimation can be achieved by nonlinear filter design relying on angular velocity measurements which are normally contaminated with noise and bias components. Likewise, it is essential that the estimator design considers any noise and/or bias components introduced during the measurement process. Furthermore, any noise component is characterized by randomness and irregular behavior. Having this in mind, one of the traditional potential functions of nonlinear deterministic complimentary filters evolved on is studied (for example [8, 19]) taking into consideration angular velocity measurements corrupted with bias and noise components. This study established that selecting the potential function in an alternative way allowed to diminish the noise. Hence, two nonlinear stochastic complementary filters on are proposed here to improve the overall estimation quality. The first stochastic filter is driven in the sense of Ito [33] and the second one is developed in the sense of Stratonovich [34]. In case when angular velocity measurement is contaminated with noise, as far as the Rodriquez vector/() is concerned, the proposed filters are able to 1) steer the error vector towards an arbitrarily small neighborhood of the origin/(identity) in probability; 2) attenuate the noise impact to a very low level for known or unknown bounded covariance; and 3) make the error semi-globally/(almost semi-globally) uniformly ultimately bounded in mean square.

The rest of the paper is organized as follows: Section II presents an overview of mathematical notation, to Rodriguez vector parameterization, and some helpful properties of the nonlinear stochastic attitude filter design. Attitude estimation dynamic problem in Rodriguez vector with Gaussian noise vector which satisfies the Brownian motion process is formulated in Section III. The nonlinear stochastic filters on and the stability analysis are presented in Section IV. Section V shows the output performance and discusses the simulation results of the proposed filters. Finally, Section VI draws a conclusion of this work.

## Ii Mathematical Notation

Throughout this paper, denotes the set of nonnegative real numbers. is the real -dimensional space while denotes the real dimensional space. For , the Euclidean norm is defined as , where is the transpose of the associated component. denotes the set of functions with continuous th partial derivatives. denotes a set of continuous and strictly increasing functions such that and vanishes only at zero. denotes a set of continuous and strictly increasing functions which belongs to class and is unbounded. denotes probability, denotes an expected value, and refers to an exponential of associated component. is the set of singular values of the associated matrix with being the minimum value. denotes identity matrix with dimension -by-, and is a zero vector with -rows and one column. denotes a potential function and for any we have and .

Let denote the 3 dimensional general linear group which is a Lie group with smooth multiplication and inversion. denotes the Special Orthogonal Group and is a subgroup of the general linear group. The attitude of a rigid-body is defined as a rotational matrix :

where is the identity matrix with -dimensions and is the determinant of the associated matrix. The associated Lie-algebra of is termed and is defined by

with being the space of skew-symmetric matrices and define the map such that

For all , we have where is the cross product between two vectors. Let the vex operator be the inverse of , denoted by such that . Let denote the anti-symmetric projection operator on the Lie-algebra , defined by such that

for all . The following two identities will be used in the subsequent derivations

(1) |

(2) |

The normalized Euclidean distance of a rotation matrix on is given by the following equation

(3) |

where denotes the trace of the associated matrix and . The attitude of a rigid-body can be constructed knowing angle of rotation and axis parameterization . This method of attitude reconstruction is termed angle-axis parameterization [9]. The mapping of angle-axis parameterization to is defined by such that

(4) |

From the other side, the attitude can be defined knowing Rodriguez parameters vector . The associated map to is given by such that

(5) |

Substituting for the rotation matrix in (5), one can further show that the normalized Euclidean distance in (3) can be expressed in terms of Rodriguez parameters:

(6) |

The anti-symmetric projection operator in square matrix space of the rotation matrix in (5) can be obtained in the sense of Rodriguez parameters vector as

It follows that the composition mapping is

(7) |

where .

## Iii Problem Formulation in Stochastic Sense

Let denote the attitude (rotational) matrix, which describes the relative orientation of the body-frame with respect to the inertial-frame as given in Fig. 1.

The attitude can be extracted from -known non-collinear inertial vectors which are measured in a coordinate system fixed to the rigid body. Let for be vectors measured in the body-fixed frame. Let , the body fixed-frame vector is defined by

(8) |

where denotes the inertial fixed-frame vector for . and denote the additive bias and noise components of the associated body-frame vector, respectively, for all . The assumption that is necessary for instantaneous three-dimensional attitude determination. In case when , the cross product of the two measured vectors can be accounted as the third vector measurement such that and . It is common to employ the normalized values of inertial and body-frame vectors in the process of attitude estimation such as

(9) |

In this manner, the attitude can be defined knowing and . Gyroscope or the rate gyros measures the angular velocity vector in the body-frame relative to the inertial-frame. The measurement vector of angular velocity is

(10) |

where denotes the true value of angular velocity, denotes an unknown constant (bias) or slowly time-varying vector, while denotes the noise component associated with angular velocity measurements, for all . The noise vector is assumed to be Gaussian. The true attitude dynamics and the associated Rodriguez vector dynamics are given in (11) and (12), respectively, as

(11) |

(12) |

In general, the measurement of angular velocity vector is subject to additive noise and bias components. These components are characterized by randomness and unknown behavior. In view of the fact that any unknown components in angular velocity measurements may impair the estimation process of the true attitude dynamics in (11) or (12), it is necessary to assume that the attitude dynamics are excited by a wide-band of random Gaussian noise process with zero mean. Combining angular velocity measurement in (10) and the attitude dynamics in (12), the attitude dynamics can be expressed as follows

(13) |

where is a bounded continuous Gaussian random noise vector with zero mean. The fact that derivative of any Gaussian process yields Gaussian process allows us to write the stochastic attitude dynamics as a function of Brownian motion process vector [35, 36]. Let be a vector process of independent Brownian motion process such that

(14) |

where is an unknown time-variant matrix with only nonzero and nonnegative bounded components in the diagonal. The covariance component associated with the noise can be defined by . The properties of Brownian motion process are defined as [33, 36, 37]

Let the attitude dynamics of Rodriguez vector in (12) be defined in the sense of Ito [33]. Considering the attitude dynamics in (13) and substituting by as in (14), the stochastic differential equation of (12) in view of (13) can be expressed by

(15) |

Similarly, the stochastic dynamics of (11) become

(16) |

where was defined in (10), and with and . is locally Lipschitz in , and is locally Lipschitz in and . Accordingly, the dynamic system in (15) has a solution for in the mean square sense and for any such that , is independent of (Theorem 4.5 [36]). Now the aim is to achieve adaptive stabilization of an unknown bias and unknown time-variant covariance matrix. Let be the upper bound of such that

(17) |

where is the maximum value of an element.

###### Assumption 1.

(Uniform boundedness of unknown parameters and ) Let the vector and the nonnegative vector belong to a given compact set where , and and are upper bounded by a scalar such that .

###### Definition 1.

Consider the stochastic differential system in (15). For a given function , the differential operator is given by

such that , and .

###### Definition 2.

###### Lemma 1.

[37, 39] Let the dynamic system in (15) be assigned a potential function such that , class function and , constants and and a nonnegative function such that

(18) |

(19) |

then for , there exists almost a unique strong solution on for the dynamic system in (15), the solution is bounded in probability such that

(20) |

Furthermore, if the inequality in (20) holds, then in (15) is SGUUB in the mean square. In addition, when , , , and is continuous, the equilibrium point is globally asymptotically stable in probability and the solution of satisfies

(21) |

The proof of this lemma and existence of a unique solution can be found in [37]. For a rotation matrix , let us define by . We have such that the set is forward invariant and unstable for the dynamic system in (11) which implies that . For almost any initial condition such that or , we have and the trajectory of is semi-globally uniformly ultimately bounded in mean square.

###### Lemma 2.

(Young’s inequality) Let and be . Then, for any and satisfying with a small positive constant , the following holds

(22) |

In the next section, the presence of noise will be examined in light of a traditional form of potential function. The concept of an alternate potential function with specific characteristics able to attenuate the noise behavior will be carefully elucidated.

## Iv Stochastic Complementary Filters On

The main goal of attitude estimation is to derive the attitude estimate . Let’s define the error in attitude estimate from the body-frame to estimator-frame by

(23) |

Let and be estimates of unknown parameters and , respectively. Define the error in vector and by

(24) | ||||

(25) |

Thus, driving ensures that and where is Rodriguez error vector associated with . In this section, two nonlinear stochastic complementary filters are developed on the Special Orthogonal Group . These filters in the sense of Rodriguez vector guarantee that the error vector is SGUUB in mean square for the case of noise contamination of the angular velocity measurements.

### Iv-a Nonlinear Deterministic Attitude Filter

In this subsection, we aim to study the behavior of nonlinear deterministic filter on with noise introduced in angular velocity measurements. The attitude can be reconstructed through a set of measurements in (9) to obtain , for instance [2, 3, 4]. is corrupted with noise and bias greatly increase the difference between and the true . The filter design aims to use the angular velocity measurements and the given to obtain good estimate of . Consider the following filter design

(26) | ||||

(27) | ||||

(28) |

where is angular velocity measurement, is the estimate of the unknown bias , and was given in (7). Also, is an adaptation gain and is a positive constant.

Let the error in vector be defined as in (24) and assume that no noise was introduced to the dynamics . The derivative of attitude error in (23) can be obtained from (11) and (26) as

(29) |

where . Hence, in view of (16) and (15), the error dynamic in (29) can be expressed in Rodriguez error vector dynamic by

(30) |

From literature, one of traditional potential functions for adaptive filter estimation is (for example [8, 19]). The equivalent of the aforementioned function in form of Rodriguez error is

(31) |

let . For , the derivative of (31) is

(32) | ||||

where which was obtained by substitution of in (5). Substituting for and in (27) and (28), respectively, yields

(33) |

Lyapunov’s direct method ensures that for , converges asymptotically to zero. As such, is an isolated equilibrium point and for [19]. If angular velocity measurements () are contaminated with noise , it is more convenient to represent the differential operator in (32) in the form of Definition 1. Hence, the following extra term will appear

In this case, the operator which implies that the significant impact of covariance matrix cannot be lessened. One way to attenuate the noise associated with the angular velocity measurements is to chose a potential function in the sense of Rodriguez error vector of order higher than two. It is worth mentioning that the deterministic filter in (26), (27) and (28) is known as a passive complementary filter proposed in [19].

### Iv-B Nonlinear Stochastic Attitude Filter in Ito Sense

Generally, the assumption behind nonlinear deterministic filters is that angular velocity vector measurements are joined with constant or slowly time-variant bias [8, 19]. However, angular velocity vector measurements are typically subject to additive noise components which may weaken the estimation process of the true attitude dynamics in (11). Therefore, we aim to design a nonlinear stochastic filter in Ito sense taking into consideration that angular velocity vector measurements are subject to a constant bias and a wide-band of Gaussian random with zero mean such that . Let the true inertial vector and body-frame vector be defined as in (8). Let the error in attitude estimate be similar to (23).

Consider the nonlinear stochastic filter design

(34) | ||||

(35) | ||||

(36) | ||||

(37) |

where is angular velocity measurement defined in (10), is the estimate of the unknown bias , is the estimate of which includes the upper bound of as given in (17), with being the reconstructed attitude, as given in (7), , and is the Euclidean distance of as defined in (3). Also, and are adaptation gains, is a small constant, while , , and are positive constants.

###### Theorem 1.

Consider the rotation dynamics in (16), angular velocity measurements in (10) in addition to other given vectorial measurements in (9) coupled with the observer (34), (35), (36), and (37). Assume that two or more body-frame non-collinear vectors are available for measurements and the design parameters , , , , , , and are chosen appropriately with being selected sufficiently small. Then, for angular velocity measurements contaminated with noise , all the signals in the closed-loop system is semi-globally uniformly ultimately bounded in mean square. In addition, the observer errors can be minimized by the appropriate selection of the design parameters.

Proof: Let the error in vector be defined as in (24). Therefore, the derivative of attitude error in incremental form of (23) can be obtained from (15) and (34) by

(38) |

Similar extraction of Rodriguez error vector dynamic in view of (16) to (15) can be expressed from (38) to (39) in Ito’s representation [33] as

(39) |

where is the Rodriguez error vector associated with . Let and . Consider the following potential function

(40) |

For , the differential operator in Definition 1 for the dynamic system in (39) can be expressed as

(41) |

where and . The first and the second partial derivatives of (40) with respect to can be obtained as follows

(42) | ||||

(43) |

substituting in (5), one can verify that

Hence, the first part of the differential operator in (41) can be evaluated by

(44) |

Keeping in mind the identity in (1) and in (39) and combining them with (43), the component can be simplified and expressed as

(45) |

Let and be similar to (17). From (44) and (45), one can write the operator in (41) as

(46) |

Since and , we have

(47) |

According to Lemma 2, the following equation holds

(48) |

where is a sufficiently small positive constant. Combining (48) with (47) yields

(49) |

Define . Substitute , , and from (35), (36), and (37), respectively, in (49). Also, and as defined in (6) and (7), respectively. Hence, (49) yields