Nonlinear Stochastic Position and Attitude Filter on the Special Euclidean Group 3

TL;DR

This paper develops a nonlinear stochastic filter directly on SE(3) for pose estimation, ensuring bounded errors and convergence despite high noise, demonstrated through simulations showing robustness and effectiveness.

Contribution

It introduces a novel stochastic pose filter on SE(3) with proven stability and convergence properties, handling high noise and bias in measurements.

Findings

Errors are semi-globally uniformly ultimately bounded in mean square.

Errors converge to a small neighborhood of the origin in probability.

Simulation results confirm robustness against high noise and bias.

Abstract

This paper formulates the pose estimation problem as nonlinear stochastic filter kinematics evolved directly on the Special Euclidean Group SE(3). Proposed filter guarantees that the errors present in position and Rodriguez vector estimates are semi-globally uniformly ultimately bounded (SGUUB) in mean square, and that they converge to small neighborhood of the origin in probability. Simulation results show the robustness and effectiveness of the proposed filter in presence of high levels of noise and bias associated with the velocity vector as well as body-frame measurements. Keywords: Pose estimator, pose observer, attitude estimate, control, estimator, observer, Nonlinear stochastic pose filter, stochastic differential equations, Brownian motion process, Ito, Stratonovich, Wong Zakai, unit-quaternion, special orthogonal group, homogeneous transformation matrix, complimentary filter, Euler angles, Angle-axis, mapping, Parameterization, Representation, Robust, Multiplicative Extended Kalman Filter, Unscented Kalman Filter, Particle filter, KF, EKF, IEKF, UKF, MEKF, partial derivative, small, dynamics, equilibrium, asymptotic, covariance, expected value, zero, unknown, time-varying, global, semi-global, stable, stability, uncertain, Gaussian, colored, white, noise, vectorial measurement, vector measurement, translational velocity, angular velocity, singular value decomposition, rotational matrix, identity, deterministic, comparison, inertial frame, rigid body, three dimensional, 3D, space, adjoint, Lie group, projection, landmark, feature, Gyroscope, micro electromechanical systems, Inertial measurement units, sensor, IMUs, Fixed, moving, orientation, Roll, Pitch, Yaw, SVD, UAVs, QUAV, unmanned, underwater vehicle, robot, Robotic System, Spacecraft, quadrotor, quadcopter, integral, advantage, disadvantage, Comparative study, Review, Overview, Survey, autonomous, xyz, axis, SO(3), SE(3).

Figures

Tables

Table 1. Table 1 : Statistical analysis of the noise and bias in input measurements and output data of the proposed filter.

Input measurements
Index	${\tilde{v}}_1^{ℬ\left(L\right)}$	${\tilde{v}}_1^{ℬ\left(R\right)}$	${\tilde{v}}_2^{ℬ\left(R\right)}$	$\tilde{\Omega }\left(\mathrm{rad}/\sec \right)$	$\tilde{V}\left(m/\sec \right)$
Mean	$\left[\begin{array}{c}0.15\\ 0.1\\ -0.1\end{array}\right]$	$\left[\begin{array}{c}-0.1\\ 0.1\\ 0.05\end{array}\right]$	$\left[\begin{array}{c}0\\ 0\\ 0.1\end{array}\right]$	$\left[\begin{array}{c}0.1\\ -0.1\\ 0.1\end{array}\right]$	$\left[\begin{array}{c}0.2\\ 0.5\\ 0.1\end{array}\right]$
STD	$0.1\times {\underset{¯}{\mathrm{𝟏}}}_3$	$0.1\times {\underset{¯}{\mathrm{𝟏}}}_3$	$0.1\times {\underset{¯}{\mathrm{𝟏}}}_3$	$0.15\times {\underset{¯}{\mathrm{𝟏}}}_3$	$0.15\times {\underset{¯}{\mathrm{𝟏}}}_3$
Output data over the period (1-30 sec)
Index	${\Vert \tilde{R}\Vert }_I$			$P-\hat{P}\left(m\right)$
Mean	$1.2\times {10}^{-3}$			${[-17.7,2.6,-8.4]}^{\top }\times {10}^{-3}$
STD	$8.5\times {10}^{-4}$			${[1.15,1.07,1.27]}^{\top }\times {10}^{-1}$

Equations

$$\mathbb{O}\left(3\right)=\left\{M\in {\mathbb{R}}^{3\times 3}|M^{\top }M=M{M}^{\top }={\mathbf{I}}_3\right\}$$

$$\mathbb{SO}\left(3\right)=\left\{R\in {\mathbb{R}}^{3\times 3}|R{R}^{\top }=R^{\top }R={\mathbf{I}}_3\text{, }\det \left(R\right)=+1\right\}$$

$$\mathbb{SE}\left(3\right)=\left\{\mathbf{T}\in {\mathbb{R}}^{4\times 4}|R\in \mathbb{SO}\left(3\right),P\in {\mathbb{R}}^3\right\}$$

$$\boldsymbol{T}=\left[\begin{array}[]{cc}R&P\\ \underline{\mathbf{0}}_{3}^{\top}&1\end{array}\right]\in\mathbb{SE}\left(3\right)$$

$$\mathfrak{so}\left(3\right)=\left\{A\in {\mathbb{R}}^{3\times 3}|A^{\top }=-A\right\}$$

$$A=\left[\alpha\right]_{\times}=\left[\begin{array}[]{ccc}0&-\alpha_{3}&\alpha_{2}\\ \alpha_{3}&0&-\alpha_{1}\\ -\alpha_{2}&\alpha_{1}&0\end{array}\right]\in\mathfrak{so}\left(3\right),\hskip 10.00002pt\alpha=\left[\begin{array}[]{c}\alpha_{1}\\ \alpha_{2}\\ \alpha_{3}\end{array}\right]$$

$$\left[\mathcal{Y}\right]_{\wedge}=\left[\begin{array}[]{cc}\left[y_{1}\right]_{\times}&y_{2}\\ \underline{\mathbf{0}}_{3}^{\top}&0\end{array}\right]\in\mathfrak{se}\left(3\right)$$

$$\mathfrak{se}\left(3\right)$$

$$\mathbf{vex}\left(A\right)=\mathbf{vex}\left({\left[\alpha \right]}_{\times }\right)=\alpha \in {\mathbb{R}}^3$$

$${\mathcal{P}}_a\left(M\right)=\frac{1}{2}\left(M-M^{\top }\right)\in \mathfrak{so}\left(3\right),M\in {\mathbb{R}}^{3\times 3}$$

$${\mathbf{{\rm Y}}}_a\left(M\right)=\mathbf{vex}\left({\mathcal{P}}_a\left(M\right)\right)\in {\mathbb{R}}^3$$

$$\boldsymbol{\mathcal{P}}\left(\mathcal{M}\right)=\boldsymbol{\mathcal{P}}\left(\left[\begin{array}[]{cc}M&m_{x}\\ m_{y}^{\top}&m_{z}\end{array}\right]\right)=\left[\begin{array}[]{cc}\boldsymbol{\mathcal{P}}_{a}\left(M\right)&m_{x}\\ \underline{\mathbf{0}}_{3}^{\top}&0\end{array}\right]\in\mathfrak{se}\left(3\right)$$

$$\boldsymbol{\Upsilon}\left(\mathcal{M}\right)=\left[\begin{array}[]{c}\boldsymbol{\Upsilon}_{a}\left(M\right)\\ m_{x}\end{array}\right]\in\mathbb{R}^{6}$$

$${\Vert R\Vert }_I=\frac{1}{4}\mathrm{Tr}\left\{{\mathbf{I}}_3-R\right\}$$

$${\mathcal{R}}_{\alpha }\left(\alpha ,u\right)={\mathbf{I}}_3+\sin \left(\alpha \right){\left[u\right]}_{\times }+\left(1-\cos \left(\alpha \right)\right){\left[u\right]}_{\times }^2\in \mathbb{SO}\left(3\right)$$

$${\mathcal{R}}_{\rho }\left(\rho \right)=$$

$${\Vert R\Vert }_I=\frac{1}{4}\mathrm{Tr}\left\{{\mathbf{I}}_3-R\right\}=\frac{{\Vert \rho \Vert }^2}{1+{\Vert \rho \Vert }^2}\in \left[0,1\right]$$

$${\mathcal{P}}_a\left(R\right)=$$

$${\mathbf{{\rm Y}}}_a\left(R\right)=\mathbf{vex}\left({\mathcal{P}}_a\left(R\right)\right)=2\frac{\rho }{1+{\Vert \rho \Vert }^2}\in {\mathbb{R}}^3$$

$${\Vert {\mathbf{{\rm Y}}}_a\left(R\right)\Vert }^2$$

$$=4\left(1-{\Vert R\Vert }_I\right){\Vert R\Vert }_I$$

$${\mathbf{Ad}}_{\mathbf{T}}\left(\mathcal{M}\right)=\mathbf{T}\mathcal{M}{\mathbf{T}}^{-1}\in \mathfrak{se}\left(3\right)$$

$$\breve{\overline{\mathbf{Ad}}}_{\boldsymbol{T}}=\left[\begin{array}[]{cc}R&\mathbf{0}_{3\times 3}\\ \left[P\right]_{\times}R&R\end{array}\right]\in\mathbb{R}^{6\times 6}$$

$$\mathbf{{\rm Y}}\left({\mathbf{Ad}}_{\mathbf{T}}\left(\mathcal{M}\right)\right)={\breve{\overline{\mathbf{Ad}}}}_{\mathbf{T}}\mathbf{{\rm Y}}\left(\mathcal{M}\right)\in {\mathbb{R}}^6$$

$$\mathbf{T}{\left[\mathcal{Y}\right]}_{\wedge }{\mathbf{T}}^{-1}={\left[{\breve{\overline{\mathbf{Ad}}}}_{\mathbf{T}}\mathcal{Y}\right]}_{\wedge }\in \mathbb{SE}\left(3\right),\mathcal{Y}\in {\mathbb{R}}^6,\mathbf{T}\in \mathbb{SE}\left(3\right)$$

$$-{\left[\beta \right]}_{\times }{\left[\alpha \right]}_{\times }$$

$${\left[R\alpha \right]}_{\times }$$

$${\breve{\overline{\mathbf{Ad}}}}_{{\mathbf{T}}_1{\mathbf{T}}_2}$$

$${\breve{\overline{\mathbf{Ad}}}}_{\mathbf{T}}{\breve{\overline{\mathbf{Ad}}}}_{{\mathbf{T}}^{-1}}$$

$$\boldsymbol{T}=\left[\begin{array}[]{cc}R&P\\ \underline{\mathbf{0}}_{3}^{\top}&1\end{array}\right]\in\mathbb{SE}\left(3\right)$$

$$\dot{P}$$

$$\dot{R}$$

$$\dot{\mathbf{T}}$$

Full text

To cite this article: \textcolor**redH. A. Hashim, L. J. Brown, and K. McIsaac, ”Nonlinear Stochastic Position and Attitude Filter on the Special Euclidean Group 3,” Journal of the Franklin Institute, vol. 356, no. 7, pp. 4144-4173, 2019.

 

**

**The published version (DOI) can be found at: 10.1016/j.jfranklin.2018.12.025

**

Please note that where the full-text provided is the preprint version** and this may differ from the revised and/or 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.

**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. **

1 Introduction

This paper concerns the problem of position and attitude estimation of a rigid-body moving in 3D space which is commonly known as the pose problem. Pose (attitude and position) estimation is a crucial task in robotics and engineering applications. The attitude and position can be reconstructed through a set of vector measurements with respect to body and inertial frames of reference. In general, the main objective of pose estimation problem is to minimize the cost function similar to Wahba’s problem [1]. The approach applied in [1] was purely algebraic, whereas other algorithms used singular value decomposition to obtain comparable static solution [2]. However, the set of vectorial measurements is susceptible to uncertainties such as slowly time-variant bias and random noise components. Therefore, the static solutions proposed in [1, 2] give poor results. Traditionally, the attitude estimation problem has been handled using Gaussian filters or nonlinear deterministic filters which aimed to converge any initialized estimate to the solution [3, 4, 5]. The family of Gaussian attitude filters includes Kalman filter (KF) [6], extended KF (EKF) [7, 8], multiplicative EKF (MEKF) [9], and invariant EKF (IEKF) [10]. A good survey of Gaussian attitude filters can be found in [4]. Gaussian attitude filters often consider the unit quaternion in attitude representation and go through liberalizations. From the other side, the attitude problem is naturally nonlinear and nonlinear deterministic attitude filters can be developed directly on the Special Orthogonal Group $\mathbb{SO}\left(3\right)$ as a deterministic problem (for example [11, 12, 13]). In fact, nonlinear deterministic attitude filters are simpler in derivation and representation. In addition, they require less computational power and demonstrate better tracking performance in comparison with Gaussian filters [4, 11, 3]. Therefore, it is better to address the attitude and attitude-position filtering in a nonlinear sense.

Inertial measurement units (IMUs) have a prominent role in enriching the research of attitude estimation. These units are inexpensive fostering the researchers to propose nonlinear deterministic filters on $\mathbb{SO}\left(3\right)$ [11, 12, 13]. Attitude estimation is an essential part of the pose estimation problem and a critical task in the estimation process. Accordingly, the pose estimation problem can be modeled and solved using a nonlinear deterministic filter evolved on the Special Euclidean Group $\mathbb{SE}\left(3\right)$. Recently, the design of pose filters received considerable attention [14, 15, 16, 17, 18, 19]. A computer vision system that employs a monocular camera with IMUs was developed for pose estimation [14, 15]. The filter in [15] evolved directly on $\mathbb{SE}\left(3\right)$ and has been proven to be exponentially stable. However, the filter requires attitude and position reconstruction for implementation. Later, the nonlinear complementary filter that evolved directly on $\mathbb{SE}\left(3\right)$ in [15] was modified using vectorial measurements without the need of attitude and position reconstruction [16, 17]. For a good overview of pose estimation on $\mathbb{SE}\left(3\right)$ the reader is advised to visit [20]. Despite the simplicity of the filter design in [15, 16, 17], simulation results showed high sensitivity to noise and bias introduced in the measurements. Moreover, pose estimators such as [14, 15, 16, 17, 18] disregard the noise in the filter design assuming only constant bias introduced in the measuring process. Therefore, successful spacecraft control applications, such as, [21, 22, 23] cannot be achieved without pose filters robust against uncertain measurements.

Therefore, in order to develop successful pose estimator, we need to realize that

the pose problem is naturally nonlinear on $\mathbb{SE}\left(3\right)$; and 2. 2)

the true pose kinematics rely on angular and translational velocity.

However, the velocity vector is subject to slowly time-variant bias and random noise components. Hence, in this work a nonlinear stochastic position and attitude filter is developed on $\mathbb{SE}\left(3\right)$ in the sense of Stratonovich [24]. The problem is mapped from $\mathbb{SE}\left(3\right)$ to vector form which includes position and Rodriquez vector such that $X:\mathbb{SE}\left(3\right)\rightarrow\mathbb{R}^{6}$. In the case where the velocity measurements are corrupted with noise, the aim is

to steer the error vectors towards an arbitrarily small neighborhood of the origin in probability; 2. 2)

to attenuate the noise impact for known or unknown bounded covariance; and 3. 3)

to show that the error in $X$ and estimates is semi-globally uniformly ultimately bounded (SGUUB) in mean square.

The rest of the paper is organized as follows: Section 2 presents an overview of mathematical notation, mapping from $\mathbb{SO}\left(3\right)$ to angle-axis and Rodriguez vector parameterization, $\mathbb{SE}\left(3\right)$ properties and some helpful properties for the nonlinear stochastic position and attitude filter design on $\mathbb{SE}\left(3\right)$. Pose estimation dynamic problem in the stochastic sense is presented in Section 3. The nonlinear stochastic filter on $\mathbb{SE}\left(3\right)$ and the stability analysis are presented in Section 4. Section 5 demonstrates numerical results and shows the output performance of the proposed stochastic filter. Finally, Section 6 draws a conclusion of this work.

2 Mathematical Notation and Background

Throughout this paper, $\mathbb{R}_{+}$ denotes the set of nonnegative real numbers. $\mathbb{R}^{n}$ is the real $n$-dimensional space, $\mathbb{R}^{n\times m}$ denotes the real $n\times m$ dimensional space. For $x\in\mathbb{R}^{n}$, the Euclidean norm is defined by $\left\|x\right\|=\sqrt{x^{\top}x}$ where ⊤ is the transpose of the associated component. $\mathcal{C}^{n}$ denotes the set of functions with continuous $n$th partial derivatives. $\mathcal{K}$ denotes a set of continuous and strictly increasing functions such that $\gamma:\mathbb{R}_{+}\rightarrow\mathbb{R}_{+}$ and vanishes only at zero. $\mathcal{K}_{\infty}$ denotes a set of continuous and strictly increasing functions which belong to class $\mathcal{K}$ and is unbounded. $\mathbb{P}\left\{\cdot\right\}$ is a probability and $\mathbb{E}\left[\cdot\right]$ is an expected value of the associated component. $\lambda\left(\cdot\right)$ is the set of singular values of associated matrix with $\underline{\lambda}\left(\cdot\right)$ being the minimum value. Also, $\mathbf{I}_{n}$ denotes identity matrix with $n$-by-$n$ dimensions, $\underline{\mathbf{0}}_{n}=\left[0,\ldots,0\right]^{\top}\in\mathbb{R}^{n}$ is a zero vector with $n$ rows and one column, and $\underline{\mathbf{1}}_{n}=\left[1,\ldots,1\right]^{\top}\in\mathbb{R}^{n}$. $V$ is a potential function, and for $V\left(x\right)$ we have $V_{x}=\partial V/\partial x$ and $V_{xx}=\partial^{2}V/\partial x^{2}$.

Notation for frames is as follows: $\left\{\mathcal{B}\right\}$ denotes the body-frame and $\left\{\mathcal{I}\right\}$ denotes the inertial-frame. Let $\mathbb{GL}\left(3\right)$ denote the 3 dimensional general linear group. $\mathbb{GL}\left(3\right)$ is a Lie group characterized by smooth multiplication and inversion. The set of orthogonal group, $\mathbb{O}\left(3\right)$ is a subgroup of the general linear group and is defined by

[TABLE]

where $\mathbf{I}_{3}$ is the identity matrix. $\mathbb{SO}\left(3\right)$ denotes the Special Orthogonal Group and is a subgroup of the orthogonal group and the general linear group. The attitude of a rigid body is denoted by a rotational matrix $R$, and is defined as follows

[TABLE]

where ${\rm det\left(\cdot\right)}$ is the determinant of the associated matrix. Let $\mathbb{SE}\left(3\right)$ denote the Special Euclidean Group with $\mathbb{SE}\left(3\right)=\mathbb{SO}\left(3\right)\times\mathbb{R}^{3}$. $\mathbb{SE}\left(3\right)$ is a subset of the affine group $\mathbb{GA}\left(3\right)=\mathbb{GL}\left(3\right)\times\mathbb{R}^{3}$ such that

[TABLE]

where $\boldsymbol{T}\in\mathbb{SE}\left(3\right)$ is known as the homogeneous representation or the transformation matrix of the rigid body and is defined by

[TABLE]

with $P\in\mathbb{R}^{3}$ denoting position, $R\in\mathbb{SO}\left(3\right)$ denoting the attitude of the rigid-body in the space, and $\underline{\mathbf{0}}_{3}^{\top}$ being a zero row. The associated Lie-algebra of $\mathbb{SO}\left(3\right)$ is termed $\mathfrak{so}\left(3\right)$ and is defined by

[TABLE]

with $A$ being the space of skew-symmetric matrices. Let us define the map $\left[\cdot\right]_{\times}:\mathbb{R}^{3}\rightarrow\mathfrak{so}\left(3\right)$ such that

[TABLE]

For all $\alpha,\beta\in\mathbb{R}^{3}$, we have $\left[\alpha\right]_{\times}\beta=\alpha\times\beta$ where $\times$ is the cross product between the two vectors. Let $\wedge$ be the wedge operator, and the wedge map $\left[\cdot\right]_{\wedge}:\mathbb{R}^{6}\rightarrow\mathfrak{se}\left(3\right)$ such that

[TABLE]

where $\mathcal{Y}=\left[y_{1}^{\top},y_{2}^{\top}\right]^{\top}$ for $y_{1},y_{2}\in\mathbb{R}^{3}$. The Lie algebra of $\mathbb{SE}\left(3\right)$ is denoted by $\mathfrak{se}\left(3\right)$ and given by

[TABLE]

Let the $\mathbf{vex}$ operator be the inverse of $\left[\cdot\right]_{\times}$, denoted by $\mathbf{vex}:\mathfrak{so}\left(3\right)\rightarrow\mathbb{R}^{3}$ such that for $\alpha\in\mathbb{R}^{3}$ and $A=\left[\alpha\right]_{\times}\in\mathfrak{so}\left(3\right)$ we have

[TABLE]

Let $\boldsymbol{\mathcal{P}}_{a}$ denote the anti-symmetric projection operator on the Lie-algebra $\mathfrak{so}\left(3\right)$, defined by $\boldsymbol{\mathcal{P}}_{a}:\mathbb{R}^{3\times 3}\rightarrow\mathfrak{so}\left(3\right)$ such that

[TABLE]

Let us define $\boldsymbol{\Upsilon}_{a}\left(\cdot\right)$ as the composition mapping such that $\boldsymbol{\Upsilon}_{a}=\mathbf{vex}\circ\boldsymbol{\mathcal{P}}_{a}$. Hence, $\boldsymbol{\Upsilon}_{a}\left(M\right)$ can be expressed for $M\in\mathbb{R}^{3\times 3}$ as

[TABLE]

Consider $\boldsymbol{\mathcal{P}}:\mathbb{R}^{4\times 4}\rightarrow\mathfrak{se}\left(3\right)$ denoting the projection operator on the space of the Lie algebra $\mathfrak{se}\left(3\right)$ such that for \mathcal{M}=\left[\begin{array}[]{cc}M&m_{x}\\ m_{y}^{\top}&m_{z}\end{array}\right]\in\mathbb{R}^{4\times 4} with $M\in\mathbb{R}^{3\times 3}$, $m_{x},m_{y}\in\mathbb{R}^{3}$ and $m_{z}\in\mathbb{R}$, we have

[TABLE]

For any $\mathcal{M}\in\mathbb{R}^{4\times 4}$, we define the operator $\boldsymbol{\Upsilon}\left(\cdot\right)$ as follows

[TABLE]

The normalized Euclidean distance of a rotation matrix on $\mathbb{SO}\left(3\right)$ is defined by

[TABLE]

such that ${\rm Tr}\left\{\cdot\right\}$ is the trace of the associated matrix, while the normalized Euclidean distance of $R\in\mathbb{SO}\left(3\right)$ is $\left\|R\right\|_{I}\in\left[0,1\right]$. The orientation of a rigid-body rotating in a 3D space can be established according to its angle of rotation $\alpha\in\mathbb{R}$ and its axis parameterization $u\in\mathbb{R}^{3}$ [25]. Such parameterization is termed angle-axis parameterization. Mapping from angle-axis parameterization to $\mathbb{SO}\left(3\right)$ is given by $\mathcal{R}_{\alpha}:\mathbb{R}\times\mathbb{R}^{3}\rightarrow\mathbb{SO}\left(3\right)$ such that

[TABLE]

In the same spirit, the orientation of a rigid-body can be constructed by Rodriguez parameters vector. Mapping from Rodriguez vector parameterization to $\mathbb{SO}\left(3\right)$ is defined by $\mathcal{R}_{\rho}:\mathbb{R}^{3}\rightarrow\mathbb{SO}\left(3\right)$ such that

[TABLE]

One can obtain the normalized Euclidean distance in (6) as a function of Rodriguez parameters vector substituting (8) into (6) to yield

[TABLE]

Also, the anti-symmetric projection operator of the attitude $R$, denoted by $\boldsymbol{\mathcal{P}}_{a}\left(R\right)$, can be defined in terms of Rodriguez parameters vector from (8) as

[TABLE]

Accordingly, the composition mapping $\boldsymbol{\Upsilon}_{a}\left(\cdot\right)$ of $\boldsymbol{\mathcal{P}}_{a}\left(R\right)$ in (2) and (3) can be defined in terms of Rodriguez parameters vector as

[TABLE]

from (9) and (10), it follows

[TABLE]

Let us consider the transformation matrix in (1) with $\boldsymbol{T}\in\mathbb{SE}\left(3\right)$. The adjoint map for any $\boldsymbol{T}\in\mathbb{SE}\left(3\right)$ and $\mathcal{M}\in\mathfrak{se}\left(3\right)$ is given by

[TABLE]

Let us define another adjoint map for any $\boldsymbol{T}\in\mathbb{SE}\left(3\right)$ by

[TABLE]

One can easily verify that the vex operator in (5) can be combined with the results in (12) and (13) to show

[TABLE]

thus

[TABLE]

which will be useful for the filter derivation and further analysis. Finally, the following identities will be used in the subsequent derivations

[TABLE]

3 Problem Formulation in Stochastic Sense

The orientation of a rigid-body rotating in 3D space $R\in\mathbb{SO}\left(3\right)$ is normally defined in terms of the body-frame $R\in\left\{\mathcal{B}\right\}$ relative to the inertial-frame $\left\{\mathcal{I}\right\}$. Let $P\in\mathbb{R}^{3}$ be the position of the rigid-body measured on the inertial-frame $P\in\left\{\mathcal{I}\right\}$. Thereby, this work concerns position as well as attitude estimation of a rigid-body moving and rotating in 3D space. Consider the homogeneous transformation matrix given by

[TABLE]

Let $\Omega\in\mathbb{R}^{3}$ and $V\in\mathbb{R}^{3}$ be angular and translational velocity of a moving rigid-body attached to the body-frame, respectively, for all $\Omega,V\in\left\{\mathcal{B}\right\}$. Hence, the dynamics of the homogeneous transformation matrix $\boldsymbol{T}$ are expressed by

[TABLE]

where $\mathcal{Y}=\left[\Omega^{\top},V^{\top}\right]^{\top}\in\mathbb{R}^{6}$ is the group velocity vector expressed relative to the body-frame. The homogeneous transformation matrix $\boldsymbol{T}$ can be reconstructed through a set of known vectors in the inertial-frame and their measurements in the body-frame. Let the superscript $\mathcal{B}$ and $\mathcal{I}$ denote the associated body-frame and inertial-frame of the component, respectively. The pose estimation problem is illustrated in Figure 1.

Assume that there exists a number of feature points or landmarks denoted by $N_{{\rm L}}$ such that

[TABLE]

with ${\rm v}_{i}^{\mathcal{B}\left({\rm L}\right)}\in\mathbb{R}^{3}$ being the landmark measurement in the body-frame and ${\rm v}_{i}^{\mathcal{I}\left({\rm L}\right)}\in\mathbb{R}^{3}$ being a known constant feature in the inertial-frame for all $i=1,\ldots,N_{{\rm L}}$. Also, ${\rm b}_{i}^{\mathcal{B}\left({\rm L}\right)}\in\mathbb{R}^{3}$ and $\omega_{i}^{\mathcal{B}\left({\rm L}\right)}\in\mathbb{R}^{3}$ are unknown bias and noise vectors attached to the $i$th measurement for all $i=1,\ldots,N_{{\rm L}}$. The position $P$ can be simply constructed if the attitude matrix $R$ is available. Let us denote the set of vectors associated with landmarks by

[TABLE]

A weighted geometric center is considered for the case of more than one landmark is available for measurement. The center is given by

[TABLE]

with $k_{i}^{{\rm L}}$ refers to the confidence level of the $i$th measurement. On the other side, the attitude matrix $R$ can be obtained through a set of $N_{{\rm R}}$-known non-collinear vectors. The $N_{{\rm R}}$ vectors are measured in the moving frame $\left\{\mathcal{B}\right\}$. Let ${\rm v}_{i}^{\mathcal{B}\left({\rm R}\right)}\in\mathbb{R}^{3}$ be a measured vector in the body-frame such that the $i$th body-frame vector is given by

[TABLE]

where ${\rm v}_{i}^{\mathcal{I}\left({\rm R}\right)}$ refers to the known vector $i$ in the inertial-frame for $i=1,2,\ldots,N_{{\rm R}}$. ${\rm b}_{i}^{\mathcal{B}\left({\rm R}\right)}$ and $\omega_{i}^{\mathcal{B}\left({\rm R}\right)}$ represent the unknown bias and noise components attached to the $i$th measurement, respectively, for all ${\rm b}_{i}^{\mathcal{B}\left({\rm R}\right)},\omega_{i}^{\mathcal{B}\left({\rm R}\right)}\in\mathbb{R}^{3}$. Let us denote the set of vectors associated with attitude reconstruction by

[TABLE]

Assumption 1

At least one feature point is available for measurements (22) with $N_{{\rm L}}\geq 1$, and three non-collinear vectors are available for measurements (26) with $N_{{\rm R}}\geq 2$. In case when $N_{{\rm R}}=2$, the third vector can be obtained by ${\rm v}_{3}^{\mathcal{I}\left({\rm R}\right)}={\rm v}_{1}^{\mathcal{I}\left({\rm R}\right)}\times{\rm v}_{2}^{\mathcal{I}\left({\rm R}\right)}$ and ${\rm v}_{3}^{\mathcal{B}\left({\rm R}\right)}={\rm v}_{1}^{\mathcal{B}\left({\rm R}\right)}\times{\rm v}_{2}^{\mathcal{B}\left({\rm R}\right)}$.

According to Assumption 1, $N_{{\rm R}}\geq 2$ means that the set of vectorial measurements in (27) is sufficient to have rank 3. The homogeneous transformation matrix $\boldsymbol{T}$ can be reconstructed if Assumption 1 is satisfied. It is common to obtain the normalized values of inertial and body-frame measurements in (26) such that

[TABLE]

and the normalized set of (28) is

[TABLE]

In that case, the attitude can be extracted knowing $\upsilon_{i}^{\mathcal{I}\left({\rm R}\right)}$ and $\upsilon_{i}^{\mathcal{B}\left({\rm R}\right)}$ instead of ${\rm v}_{i}^{\mathcal{I}\left({\rm R}\right)}$ and ${\rm v}_{i}^{\mathcal{B}\left({\rm R}\right)}$. Gyroscope obtains the measurements of angular velocity in the body-frame $\left\{\mathcal{B}\right\}$ and the measurement vector is defined by

[TABLE]

with $\Omega$ denoting the true value of angular velocity, $b_{\Omega}\in\mathbb{R}^{3}$ denoting the bias component which is unknown constant or slowly time-varying vector, and $\omega_{\Omega}\in\mathbb{R}^{3}$ being the unknown noise component attached to angular velocity measurements. Also, the translational velocity is expressed in the body-frame and its measurement is defined by

[TABLE]

where $V$ denotes the true value of the translational velocity, $b_{V}\in\mathbb{R}^{3}$ denotes the unknown bias component, and $\omega_{V}\in\mathbb{R}^{3}$ is the unknown noise component attached to translational velocity measurements. Let the group of velocity measurements, bias and noise vectors be defined by $\mathcal{Y}_{m}=\left[\Omega_{m}^{\top},V_{m}^{\top}\right]^{\top}$, $b=\left[b_{\Omega}^{\top},b_{V}^{\top}\right]^{\top}$ and $\omega=\left[\omega_{\Omega}^{\top},\omega_{V}^{\top}\right]^{\top}$, respectively, for all $\mathcal{Y}_{m},b,\omega\in\mathbb{R}^{6}$. The noise vector $\omega$ is assumed to be Gaussian with zero mean. The dynamics of (20) can be mapped to Rodriguez vector and expressed as follows [25]

[TABLE]

Therefore, the dynamics of the homogeneous transformation matrix in (21) can be mapped to vector form in the sense of Rodriguez parameters from (32) and (8) as

[TABLE]

where $\mathcal{R}_{\rho}\left(\rho\right)=R\in\mathbb{SO}\left(3\right)$ as given in (8). According to (30) and (31), the measurements of angular and translational velocities are subject to noise and bias components. These components are characterized by randomness and uncertainty. As such, random behavior and the randomness in measurements could lead to unknown behavior [3, 26, 27] and impair the whole estimation process. The dynamics of the homogeneous transformation matrix in (21) become

[TABLE]

In view of (21) and (33), the dynamics in (34) can be mapped in the same sense and represented as

[TABLE]

where $\omega$ is a continuous Gaussian random noise vector with zero mean which is bounded. The derivative of any Gaussian process yields a Gaussian process [28, 29]. Hence, the vector $\omega$ can be written as a function of Brownian motion process vector $d\beta/dt$ with $\beta\in\mathbb{R}^{6}$ such that

[TABLE]

where $\beta=\left[\beta_{\Omega}^{\top},\beta_{V}^{\top}\right]^{\top}$ and $\mathcal{Q}\in\mathbb{R}^{6\times 6}$ is a diagonal matrix whose diagonal has unknown time-variant nonnegative components defined by

[TABLE]

where $\mathcal{Q}_{\Omega}\in\mathbb{R}^{3\times 3}$ is associated with $\omega_{\Omega}$ and $\mathcal{Q}_{V}\in\mathbb{R}^{3\times 3}$ is associated with $\omega_{V}$. In addition, $\mathcal{Q}^{2}=\mathcal{Q}\mathcal{Q}^{\top}$ is a covariance component associated with the noise vector $\omega$. The properties of Brownian motion process are defined by [29, 30, 31]

[TABLE]

Let the dynamics of the homogeneous transformation in (21) be defined in the sense of Stratonovich [24] and substitute $\omega$ by $\mathcal{Q}d\beta/dt$. Accordingly, the stochastic differential equation of (21) can be expressed as

[TABLE]

in view of (34) and (35), the stochastic differential equation in (36) is given by

[TABLE]

Let us define

[TABLE]

with $X=\left[\rho^{\top},P^{\top}\right]^{\top}\in\mathbb{R}^{6}$, $\mathcal{G}:\mathbb{R}^{3}\rightarrow\mathbb{R}^{6\times 6}$ and $f:\mathbb{R}^{3}\times\mathbb{R}^{6}\rightarrow\mathbb{R}^{6}$. $\mathcal{G}\left(\rho\right)$ is locally Lipschitz in $\rho$ and $f\left(\rho,b\right)$ is locally Lipschitz in $\rho$ and $b$. Consequently, the dynamic system in (41) has a solution on $t\in\left[t\left(0\right),T\right]\forall t\left(0\right)\leq T<\infty$ in the mean square sense and for any $\rho\left(t\right)$ and $P\left(t\right)$ such that $t\neq t\left(0\right)$, $X-X\left(0\right)$ is independent of $\left\{\beta\left(\tau\right),\tau\geq t\right\},\forall t\in\left[t\left(0\right),T\right]$ (Theorem 4.5 [29]). The aim is to achieve adaptive stabilization of an unknown constant bias and unknown time-variant covariance matrix. Let $\sigma=\left[\sigma_{\Omega}^{\top},\sigma_{V}^{\top}\right]^{\top}\in\mathbb{R}^{6}$ with $\sigma_{\Omega},\sigma_{V}\in\mathbb{R}^{3}$ being the upper bound of $\mathcal{Q}^{2}$ such that

[TABLE]

where ${\rm max}\left\{\cdot\right\}$ is the maximum value of the associated covariance element.

Assumption 2

Both $b$ and $\sigma$ belong to a given compact set $\Delta$ and are upper bounded by a scalar $\Gamma$ such that $\left\|\Delta\right\|\leq\Gamma<\infty$.

Definition 1

[32]** The trajectory $X=\left[\rho^{\top},P^{\top}\right]^{\top}$ of the stochastic differential system in (41) is said to be semi-globally uniformly ultimately bounded (SGUUB) if for some compact set $\Xi\in\mathbb{R}^{6}$ and any $X\left(0\right)=X\left(t\left(0\right)\right)$, there exists a constant $\vartheta>0$, and a time constant $T=T\left(\vartheta,X\left(0\right)\right)$ such that $\mathbb{E}\left[\left\|X\right\|\right]<\vartheta,\forall t>t\left(0\right)+T$.

Definition 2

Consider the stochastic differential system in (41) with $X=\left[\rho^{\top},P^{\top}\right]^{\top}$. For a given function $V\left(X\right)\in\mathcal{C}^{2}$ the differential operator $\mathcal{L}V$ is given by

[TABLE]

such that $V_{X}=\partial V/\partial X$, and $V_{XX}=\partial^{2}V/\partial X^{2}$.

Lemma 1

[31, 32, 33]** Consider the dynamic system in (41) with potential function $V\in\mathcal{C}^{2}$, such that $V:\mathbb{R}^{6}\rightarrow\mathbb{R}_{+}$, class $\mathcal{K}_{\infty}$ function $\bar{\alpha}_{1}\left(\cdot\right)$ and $\bar{\alpha}_{2}\left(\cdot\right)$, constants $c_{1}>0$ and $c_{2}\geq 0$ and a nonnegative function $\mathbf{Z}\left(\left\|X\right\|\right)$ such that

[TABLE]

[TABLE]

then for $X\left(0\right)\in\mathbb{R}^{6}$, there exists almost a unique strong solution on $\left[0,\infty\right)$ for the dynamic system in (41). The solution $X$ is bounded in probability such that

[TABLE]

Moreover, if the inequality in (51) holds, then $X$ in (41) is SGUUB in the mean square. Also, when $c_{2}=0$, $f\left(0,b\right)=0$, $\mathcal{G}\left(0\right)=0$, and $\mathbf{Z}\left(\left\|X\right\|\right)$ is continuous, the equilibrium point $X=0$ is globally asymptotically stable in probability and the solution of $X$ satisfies

[TABLE]

The proof of this lemma and the existence of a unique solution can be found in [31]. For a rotation matrix $R\in\mathbb{SO}\left(3\right)$, let us define $\mathcal{U}_{0}\subseteq\mathbb{SO}\left(3\right)\times\mathbb{R}^{3}$ by $\mathcal{U}_{0}=\left\{\left.\left(R\left(0\right),P\left(0\right)\right)\right|{\rm Tr}\left\{R\left(0\right)\right\}=-1,P\left(0\right)=\underline{\mathbf{0}}_{3}\right\}$. The set $\mathcal{U}_{0}$ is forward invariant and unstable for the dynamic system (20) and (21), as ${\rm Tr}\left\{R\left(0\right)\right\}=-1$ implies $\rho\left(0\right)=\infty$ [25, 3]. From almost any initial condition such that $R\left(0\right)\notin\mathcal{U}_{0}$ or equivalently $\rho\left(0\right)\in\mathbb{R}^{3}$, we have $-1<{\rm Tr}\left\{R\left(0\right)\right\}\leq 3$ and the trajectory of $X=\left[\rho^{\top},P^{\top}\right]^{\top}$ converges to the neighborhood of the equilibrium point conditioned on the value of $c_{2}$ in (50).

Lemma 2

(Young’s inequality) Let $x$ and $y$ be real values such that $x,y\in\mathbb{R}^{3}$. Then, for any positive real numbers $c$ and $d$ satisfying $\frac{1}{c}+\frac{1}{d}=1$ with appropriately small positive constant $\varepsilon$, the following inequality holds

[TABLE]

4 Nonlinear Stochastic Complementary Filter on $\mathbb{SE}\left(3\right)$

Let $\hat{\boldsymbol{T}}$ be the estimator of the homogeneous transformation matrix $\boldsymbol{T}$ such that

[TABLE]

The main purpose of this section is to design a pose estimator to drive $\hat{\boldsymbol{T}}\rightarrow\boldsymbol{T}$. Let us define the error in the estimation of the homogeneous transformation matrix by

[TABLE]

with $\tilde{R}=R\hat{R}^{\top}$ and $\tilde{P}=P-\tilde{R}\hat{P}$. Driving $\hat{\boldsymbol{T}}\rightarrow\boldsymbol{T}$ guarantees that $\tilde{P}\rightarrow 0$ and $\tilde{\rho}\rightarrow 0$, where $\tilde{P}$ is the position error associated with $\tilde{\boldsymbol{T}}$ and $\tilde{\rho}$ is the error of Rodriguez vector associated with $\tilde{R}$ which is in turn associated with $\tilde{\boldsymbol{T}}$. In this Section, a nonlinear deterministic filter on $\mathbb{SE}\left(3\right)$ is presented. This filter is subsequently modified into a nonlinear stochastic filter evolved directly on $\mathbb{SE}\left(3\right)$. The nonlinear stochastic filter is driven in the sense of Stratonovich. For $\tilde{X}=\left[\tilde{\rho}^{\top},\tilde{P}^{\top}\right]^{\top}\in\mathbb{R}^{6}$, the error vector $\tilde{X}$ is regulated to an arbitrarily small neighborhood of the origin in the case where velocity vector measurements $\mathcal{Y}_{m}$ are contaminated with constant bias and random noise at each time instant. Let $\hat{b}$ and $\hat{\sigma}$ denote estimates of unknown parameters $b$, and $\sigma$, respectively. Let the error in vector $b$ and $\sigma$ be defined by

[TABLE]

4.1 Nonlinear Deterministic Pose Filter

The aim of this subsection is to study the behavior of nonlinear deterministic pose filter evolved directly on $\mathbb{SE}\left(3\right)$ in presence of noise in the velocity vector measurements $\mathcal{Y}_{m}$. The attitude can be constructed algebraically given a set of measurements in (27) to form $R_{y}$, for example [1, 2]. However, $R_{y}$ is uncertain and significantly far from the true $R$. The given set of measurements in (29) helps in finding $R_{y}$ and for a given landmark(s) we have $P_{y}=\frac{1}{\sum_{i=1}^{N_{{\rm L}}}k_{i}^{{\rm L}}}\sum_{i=1}^{N_{{\rm L}}}k_{i}^{{\rm L}}\left({\rm v}_{i}^{\mathcal{I}\left({\rm L}\right)}-R_{y}{\rm v}_{i}^{\mathcal{B}\left({\rm L}\right)}\right)$ and \boldsymbol{T}_{y}=\left[\begin{array}[]{cc}R_{y}&P_{y}\\ \underline{\mathbf{0}}_{3}^{\top}&1\end{array}\right]. Hence, the filter design aims to use the given measured $\boldsymbol{T}_{y}$, and the velocity measurements in (30), and (31) to obtain a good estimate of the true $\boldsymbol{T}$. Consider the nonlinear deterministic pose filter design

[TABLE]

where $\mathcal{Y}_{m}=\left[\Omega_{m}^{\top},V_{m}^{\top}\right]^{\top}$ is a measured vector of angular and translational velocity defined in (30) and (31), respectively, with no noise attached to measurements $\left(\omega=0\right)$. $\hat{b}=\left[\hat{b}_{\Omega}^{\top},\hat{b}_{V}^{\top}\right]^{\top}\in\mathbb{R}^{6}$ is the estimate of the unknown bias vector $b$, $\tilde{\boldsymbol{T}}=\boldsymbol{T}_{y}\hat{\boldsymbol{T}}^{-1}$, $\boldsymbol{\Upsilon}\left(\tilde{\boldsymbol{T}}\right)=\left[\boldsymbol{\Upsilon}_{a}^{\top}\left(\tilde{R}\right),\tilde{P}^{\top}\right]^{\top}$ as in (5), $\boldsymbol{\Upsilon}_{a}\left(\tilde{R}\right)=\mathbf{vex}\left(\boldsymbol{\mathcal{P}}_{a}\left(\tilde{R}\right)\right)$, and $\left\|\tilde{R}\right\|_{I}=\frac{1}{4}{\rm Tr}\left\{\mathbf{I}_{3}-\tilde{R}\right\}$. Also, \breve{\overline{\mathbf{Ad}}}_{\hat{\boldsymbol{T}}}=\left[\begin{array}[]{cc}\hat{R}&\mathbf{0}_{3\times 3}\\ \left[\hat{P}\right]_{\times}\hat{R}&\hat{R}\end{array}\right], \Gamma=\left[\begin{array}[]{cc}\Gamma_{\Omega}&\mathbf{0}_{3\times 3}\\ \mathbf{0}_{3\times 3}&\Gamma_{V}\end{array}\right]=\gamma\mathbf{I}_{6}, is an adaptation gain with $\Gamma_{\Omega},\Gamma_{V}\in\mathbb{R}^{3\times 3}$, $\gamma>0$, and $k_{b}$, $k_{p}$ and $k_{w}$ are positive constants.

Theorem 1

Consider the homogeneous transformation matrix dynamics in (21) with velocity measurements $\mathcal{Y}_{m}$ in (30) and (31). Let Assumption 1 hold and assume that the vector measurements in (26) are normalized to (28). Let $\boldsymbol{T}_{y}$ be reconstructed using the vector measurement in (22) and (28) , and be coupled with the observer in (61), (64) and (67). In case when velocity vector measurements $\mathcal{Y}_{m}$ are subject to constant bias, no noise is introduced to the system $\left(\omega=0\right)$, $\tilde{X}\left(0\right)=\left[\tilde{\rho}\left(0\right)^{\top},\tilde{P}\left(0\right)^{\top}\right]^{\top}\in\mathbb{R}^{6}$, and $\tilde{X}\left(0\right)\neq\underline{\mathbf{0}}_{6}$, 1) the error vector $\tilde{X}$ is uniformly ultimately bounded for all $t\geq t\left(0\right)$; and 2) consequently $\left(\tilde{\boldsymbol{T}},\tilde{b}\right)$ steers to the neighborhood of the equilibrium set $\mathcal{S}=\left\{\left(\tilde{\boldsymbol{T}},\tilde{b}\right)\in\mathbb{SE}\left(3\right)\times\mathbb{R}^{6}:\tilde{\boldsymbol{T}}=\mathbf{I}_{4},\tilde{b}=\underline{\mathbf{0}}_{6}\right\}$.

Proof. Let the error in $b$ and $\tilde{\boldsymbol{T}}$ be defined as in (59), and (58), respectively. Therefore, the derivative of homogeneous transformation matrix error in (58) can be expressed from (34) and (61) as

[TABLE]

where $\dot{\hat{\boldsymbol{T}}}^{-1}=-\hat{\boldsymbol{T}}^{-1}\dot{\hat{\boldsymbol{T}}}\hat{\boldsymbol{T}}^{-1}$, and $\tilde{b}=\left[\tilde{b}_{\Omega}^{\top},\tilde{b}_{V}^{\top}\right]^{\top}$. Considering the math identity in (14), we have $\hat{\boldsymbol{T}}\left[\tilde{b}\right]_{\wedge}\hat{\boldsymbol{T}}^{-1}=\left[\breve{\overline{\mathbf{Ad}}}_{\hat{\boldsymbol{T}}}\tilde{b}\right]_{\wedge}$. For $\tilde{X}=\left[\tilde{\rho}^{\top},\tilde{P}^{\top}\right]^{\top}$, and in view of the transformation of (34) into (35), one may write (68) as

[TABLE]

with

[TABLE]

and $\mathcal{R}_{\tilde{\rho}}\left(\tilde{\rho}\right)=\tilde{R}\in\mathbb{SO}\left(3\right)$ as given in (8). Consider the following potential function

[TABLE]

for $V:=V\left(\tilde{\rho},\tilde{P},\tilde{b}\right)$ the derivative of (70) is defined by

[TABLE]

substitute for $\left\|\tilde{R}\right\|_{I}=\left\|\tilde{\rho}\right\|^{2}/\left(1+\left\|\tilde{\rho}\right\|^{2}\right)$ and $\boldsymbol{\Upsilon}_{a}\left(\tilde{R}\right)=2\tilde{\rho}/\left(1+\left\|\tilde{\rho}\right\|^{2}\right)$ from (6) and (10), respectively, the result in (76) becomes

[TABLE]

such that $\boldsymbol{\Upsilon}\left(\tilde{\boldsymbol{T}}\right)=\left[\boldsymbol{\Upsilon}_{a}^{\top}\left(\tilde{R}\right),\tilde{P}^{\top}\right]^{\top}$, substituting for $\dot{\hat{b}}$ and $W$ from (64) and (67), respectively, with $\left\|\boldsymbol{\Upsilon}_{a}\left(\tilde{R}\right)\right\|^{2}=4\left(1-\left\|\tilde{R}\right\|_{I}\right)\left\|\tilde{R}\right\|_{I}=4\frac{\left\|\tilde{\rho}\right\|^{2}}{\left(1+\left\|\tilde{\rho}\right\|^{2}\right)^{2}}$ as in (11) yields

[TABLE]

applying Young’s inequality to $k_{b}\tilde{b}^{\top}b$, one obtains $k_{b}\tilde{b}^{\top}b\leq\frac{k_{b}}{2}\left\|\tilde{b}\right\|^{2}+\frac{k_{b}}{2}\left\|b\right\|^{2}$. Define

[TABLE]

therefore, equation (80) becomes

[TABLE]

Let $c_{1}=\underline{\lambda}\left(\mathcal{H}\right)$ and $c_{2}=\frac{k_{b}}{2}\left\|b\right\|^{2}$, thus, the result in (81) implies that $\hat{X}$ and $\hat{b}$ will eventually converge to the compact set

[TABLE]

with

[TABLE]

and

[TABLE]

The result obtained in (81) is similar to Lemma 1.2 in [34] which confirms the result in Theorem 1. Theorem 1 is developed for deterministic observers, assuming absence of noises in the system dynamics. Hence, Lyapunov’s direct method guarantees that for ${\rm Tr}\left\{\tilde{R}\left(0\right)\right\}\neq-1$, $\boldsymbol{\Upsilon}\left(\tilde{\boldsymbol{T}}\right)$ converges to a small neighborhood of the origin. However, if the velocity vector $\mathcal{Y}_{m}$ is contaminated with noise such that $\left(\omega\neq 0\right)$, it would no longer be convenient to express the derivative of (70) similar to (76). Therefore, the derivative of (70) should be expressed analogously to the differential operator in Definition 2 and consequently, the covariance matrix $\mathcal{Q}^{2}$ appears there. As a result, one solution is to reformulate the potential function in (70) such that $\tilde{\rho}$ and $\tilde{P}$ are of order higher than two [31, 33]. Clearly, this is not the case in Theorem 1 as well as in previous studies such as [14, 15, 16, 17, 18].

4.2 Nonlinear Stochastic Pose Filter in Stratonovich Sense

Generally, nonlinear deterministic attitude or attitude-position filters assume that velocity measurements are subject only to constant bias (for example [4, 11, 14, 15, 16, 17]). In contrast, the velocity vector $\mathcal{Y}_{m}$ is contaminated not only with bias but also noise components. The added components could impair the estimation process of the true position and attitude. As such, the aim is to design a nonlinear stochastic filter evolved directly on $\mathbb{SE}\left(3\right)$ in the sense of Stratonovich [24] considering that measurement in the velocity vector $\mathcal{Y}_{m}$ is contaminated with constant bias and a wide-band of Gaussian random noise with zero mean. Stochastic differential equations can be defined and solved in the sense of Ito’s integral [30]. Alternatively, Stratonovich’s integral [24] can be employed for solving stochastic differential equations. The common feature between Stratonovich and Ito integral is that if the associated function multiplied by $d\beta$ is continuous and Lipschitz, the mean square limit exists. The Ito integral is defined for functional on $\left\{\beta\left(\tau\right),\tau\leq t\right\}$ which is more natural but it does not obey the chain rule. Conversely, Stratonovich is a well-defined Riemann integral for the sampled function, it has a continuous partial derivative with respect to $\beta$, it obeys the chain rule, and it is more convenient for colored noise [24, 29]. Hence, the Stratonovich integral is defined for explicit functions of $\beta$. In case of a wide-band of random colored noise process being attached to the velocity measurements, for $X=\left[\rho^{\top},P^{\top}\right]^{\top}$ with $X\left(t_{0}\right)=0$, the solution of (41) is defined by

[TABLE]

if the problem has been considered and solved directly in the sense of Ito, the expected value of (82) is

[TABLE]

Hence, Stratonovich came up with the Wong-Zakai correction factor to balance any colored noise that may be introduced to the system dynamics and to end with $\mathbb{E}\left[X\right]=\int_{t_{0}}^{t}\mathbb{E}\left[f\left(\rho,b\right)\right]d\tau$. A remarkable advantage of Stratonovich is its applicability to white noise as well as colored noise which makes the filter more robust for real time applications [24, 28, 29]. Let us assume that the attitude dynamics in (41) were defined in the sense of Stratonovich [24]. Therefore, the equivalent Ito [28, 29, 30] can be expressed as

[TABLE]

where both $f\left(\rho,b\right)$ and $\mathcal{G}\left(\rho\right)$ are defined in (41). $\sum_{k=1}^{6}\sum_{j=1}^{6}\frac{\mathcal{Q}_{j,j}^{2}}{2}\mathcal{G}_{kj}\left(\rho\right)\frac{\partial\mathcal{G}_{ij}\left(\rho\right)}{\partial\rho_{k}}$ is termed the Wong-Zakai correction factor of stochastic differential equations (SDEs) in the sense of Ito [35], and $i,j,k=1,\ldots,6$ denote $i$th, $j$th and/or $k$th elements of the associated vector or matrix. Assume that $\boldsymbol{\mathcal{W}}\left(\rho\right)=\left[\boldsymbol{\mathcal{W}}_{\rho}^{\top},\boldsymbol{\mathcal{W}}_{P}^{\top}\right]^{\top}\in\mathbb{R}^{6}$. Let $\boldsymbol{\mathcal{W}}_{\rho i}=\sum_{k=1}^{3}\sum_{j=1}^{3}\frac{\mathcal{Q}_{j,j}^{2}}{2}\mathcal{G}_{kj}\left(\rho\right)\frac{\partial\mathcal{G}_{ij}\left(\rho\right)}{\partial\rho_{k}}$, therefore, for $i=1$

[TABLE]

Thus, one can find that for $i=1,2,3$, $\boldsymbol{\mathcal{W}}_{\rho}\in\mathbb{R}^{3}$ can be defined after some steps of calculations as follows

[TABLE]

And $\boldsymbol{\mathcal{W}}_{Pi}=\sum_{k=4}^{6}\sum_{j=4}^{6}\frac{\mathcal{Q}_{j,j}^{2}}{2}\mathcal{G}_{kj}\left(\rho\right)\frac{\partial\mathcal{G}_{ij}\left(\rho\right)}{\partial P_{k}}=0$, for $i=4,5,6$. This implies that

[TABLE]

Manipulating equations (83), (84) and (85), the stochastic dynamics of the Rodriguez vector can be expressed as

[TABLE]

Assume that the elements of covariance matrix $\mathcal{Q}^{2}$ are upper bounded by $\sigma$ as given in (48) such that the bound of $\sigma$ is unknown for nonnegative elements. Consider the nonlinear stochastic pose filter design

[TABLE]

where $\mathcal{Y}_{m}=\left[\Omega_{m}^{\top},V_{m}^{\top}\right]^{\top}$ denotes the measured vector of angular and translational velocity defined in (30) and (31), respectively. $\hat{b}=\left[\hat{b}_{\Omega}^{\top},\hat{b}_{V}^{\top}\right]^{\top}\in\mathbb{R}^{6}$ and $\hat{\sigma}=\left[\hat{\sigma}_{\Omega}^{\top},\hat{\sigma}_{V}^{\top}\right]^{\top}\in\mathbb{R}^{6}$ are estimates of the unknown parameter $b$ and $\sigma$, respectively, $\tilde{\boldsymbol{T}}=\boldsymbol{T}_{y}\hat{\boldsymbol{T}}^{-1}$, $\boldsymbol{\Upsilon}\left(\tilde{\boldsymbol{T}}\right)=\left[\boldsymbol{\Upsilon}_{a}^{\top}\left(\tilde{R}\right),\tilde{P}^{\top}\right]^{\top}$ as in (5), $\boldsymbol{\Upsilon}_{a}\left(\tilde{R}\right)=\mathbf{vex}\left(\boldsymbol{\mathcal{P}}_{a}\left(\tilde{R}\right)\right)$ as given in (10), $\left\|\tilde{R}\right\|_{I}=\frac{1}{4}{\rm Tr}\left\{\mathbf{I}_{3}-\tilde{R}\right\}$ is the Euclidean distance of $\tilde{R}$ as defined in (6), and $\mathcal{D}_{\Upsilon}=\left[\boldsymbol{\Upsilon}_{a}\left(\tilde{R}\right),\boldsymbol{\Upsilon}_{a}\left(\tilde{R}\right),\boldsymbol{\Upsilon}_{a}\left(\tilde{R}\right)\right]$. Also, \breve{\overline{\mathbf{Ad}}}_{\hat{\boldsymbol{T}}}=\left[\begin{array}[]{cc}\hat{R}&\mathbf{0}_{3\times 3}\\ \left[\hat{P}\right]_{\times}\hat{R}&\hat{R}\end{array}\right], \Gamma=\left[\begin{array}[]{cc}\Gamma_{\Omega}&\mathbf{0}_{3\times 3}\\ \mathbf{0}_{3\times 3}&\Gamma_{V}\end{array}\right]=\gamma\mathbf{I}_{6}, and \Pi=\left[\begin{array}[]{cc}\Pi_{\Omega}&\mathbf{0}_{3\times 3}\\ \mathbf{0}_{3\times 3}&\Pi_{V}\end{array}\right]=\bar{\pi}\mathbf{I}_{6} are adaptation gains with $\Gamma_{\Omega},\Gamma_{V},\Pi_{\Omega},\Pi_{V}\in\mathbb{R}^{3\times 3}$ where $\gamma,\bar{\pi}>0$, $\varepsilon>0$ is a small constant, and $k_{b}$, $k_{\sigma}$, $k_{p}$ and $k_{w}$ are positive constants.

Theorem 2

*Consider the homogeneous transformation matrix dynamics in (21) with velocity measurements $\mathcal{Y}_{m}=\left[\Omega_{m}^{\top},V_{m}^{\top}\right]^{\top}$ in (30) and (31). Let Assumption 1 hold and assume that the vector measurements in (26) are normalized to (28). Let $\boldsymbol{T}_{y}$ be reconstructed using the vector measurements in (22) and (28), and be coupled with the observer in (89), (92), (97) and (102). Assume the design parameters $\Gamma$, $\Pi$, $\varepsilon$, $k_{b}$, $k_{\sigma}$, $k_{p}$ and $k_{w}$ are chosen appropriately with $\varepsilon$ being selected sufficiently small. When velocity measurements $\mathcal{Y}_{m}$ are contaminated with bias and noise $\left(\omega\neq 0\right)$, $\tilde{X}\left(0\right)=\left[\tilde{\rho}\left(0\right)^{\top},\tilde{P}\left(0\right)^{\top}\right]^{\top}\in\mathbb{R}^{6}$, and $\tilde{X}\left(0\right)\neq\underline{\mathbf{0}}_{6}$, then

1. the errors $\left(\tilde{\boldsymbol{T}},\tilde{b},\tilde{\sigma}\right)$ are regulated to the neighborhood of the equilibrium set $\mathcal{S}=\left\{\left(\tilde{\boldsymbol{T}},\tilde{b},\tilde{\sigma}\right)\in\mathbb{SE}\left(3\right)\times\mathbb{R}^{6}\times\mathbb{R}^{6}:\tilde{\boldsymbol{T}}=\mathbf{I}_{4},\tilde{b}=\underline{\mathbf{0}}_{6},\tilde{\sigma}=\underline{\mathbf{0}}_{6}\right\}$; and 2) $\left[\tilde{X}^{\top},\tilde{b}^{\top},\tilde{\sigma}^{\top}\right]^{\top}$is semi-globally uniformly ultimately bounded in mean square.*

**Proof: **Let the error in the homogeneous transformation matrix $\boldsymbol{T}$ be given as in (58) and the error in vector $b$ be defined as in (59). Therefore, the derivative of homogeneous transformation matrix error $\tilde{\boldsymbol{T}}$ in (58) in incremental form can be obtained from (34) and (89) by

[TABLE]

where $\dot{\hat{\boldsymbol{T}}}^{-1}=-\hat{\boldsymbol{T}}^{-1}\dot{\hat{\boldsymbol{T}}}\hat{\boldsymbol{T}}^{-1}$, and $\tilde{b}=\left[\tilde{b}_{\Omega}^{\top},\tilde{b}_{V}^{\top}\right]^{\top}$. Considering the math identity in (14) we have $\hat{\boldsymbol{T}}\left[\tilde{b}\right]_{\wedge}\hat{\boldsymbol{T}}^{-1}=\left[\breve{\overline{\mathbf{Ad}}}_{\hat{\boldsymbol{T}}}\tilde{b}\right]_{\wedge}$, and from the math identity in (17) and (18), we have $\breve{\overline{\mathbf{Ad}}}_{\hat{\boldsymbol{T}}}\breve{\overline{\mathbf{Ad}}}_{\hat{\boldsymbol{T}}^{-1}}=\mathbf{I}_{6}$. Similarly to transition from (36) to (41), extraction of vector dynamics in (111) can be expressed as (119) and (120) in Stratonovich’s representation [24] as follows

[TABLE]

Or more simply as

[TABLE]

where

[TABLE]

and

[TABLE]

One can re-define

[TABLE]

for all $\bar{\omega}_{\Omega},\bar{\omega}_{V}\in\mathbb{R}^{3}$ such that

[TABLE]

with

[TABLE]

Thus, the dynamics in (111) and (120) can be re-expressed, respectively, as

[TABLE]

[TABLE]

Hence, in view of (83) and (86), the error dynamics in (124) can be re-expressed in the sense of Ito [30, 3] as

[TABLE]

with $\boldsymbol{\mathcal{W}}_{\tilde{\rho}}=\frac{1}{4}\left(\mathbf{I}_{3}+\left[\tilde{\rho}\right]_{\times}+\tilde{\rho}\tilde{\rho}^{\top}\right)\mathcal{\bar{Q}}_{\Omega}^{2}\tilde{\rho}$ and $\boldsymbol{\mathcal{W}}_{\tilde{P}}=\underline{\mathbf{0}}_{3}$ as defined in (84) and (85), respectively, which can be further simplified as shown below

[TABLE]

where $\mathcal{F}=\left[\mathcal{F}_{\tilde{\rho}}^{\top},\mathcal{F}_{\tilde{P}}^{\top}\right]^{\top}=-f_{\tilde{X}}+\boldsymbol{\mathcal{W}}\left(\tilde{\rho}\right)$. Let us re-define $\sigma$ as the upper bound of $\bar{\mathcal{Q}}^{2}$ with $\sigma=\left[\sigma_{\Omega}^{\top},\sigma_{V}^{\top}\right]^{\top}\in\mathbb{R}^{6}$ and $\sigma_{\Omega},\sigma_{V}\in\mathbb{R}^{3}$ such that

[TABLE]

Let the error in $\sigma$ be defined similar to (60) with $\tilde{\sigma}=\sigma-\hat{\sigma}$. Consider the following potential function

[TABLE]

For $V:=V\left(\tilde{\rho},\tilde{P},\tilde{b},\tilde{\sigma}\right)$, the differential operator $\mathcal{L}V$ in Definition 2 can be written as

[TABLE]

One can easily show that the first and second partial derivatives of (132) in terms of $\tilde{\rho}$ can be obtained as follows

[TABLE]

Similarly, the first and second partial derivatives of (132) in terms of $\tilde{P}$ can be obtained as follows

[TABLE]

The first part of the differential operator $\mathcal{L}V$ in (133) can be evaluated by

[TABLE]

Hence, the differential operator $\mathcal{L}V$ in (133) can be described by

[TABLE]

where $\frac{1}{4}\frac{\boldsymbol{\Upsilon}_{a}\left(\tilde{R}\right)}{1-\left\|\tilde{R}\right\|_{I}}=\frac{1}{2}\tilde{\rho}$ as given in (9) and (10). Now, let us simplify the trace bracket in (144). To simplify the result in (144), one has

[TABLE]

and for

[TABLE]

we have

[TABLE]

Similarly, one can find

[TABLE]

and for

[TABLE]

we have

[TABLE]

Hence, the operator in (144) becomes

[TABLE]

According to Lemma 2, the following two equations hold

[TABLE]

[TABLE]

Considering the results in (151) and (152), in addition, $\left(\sum_{i=1}^{6}\sigma_{i}\right)^{2}\geq\left(\sum_{i=1}^{3}\sigma_{i}\right)^{2}+\left(\sum_{i=4}^{6}\sigma_{i}\right)^{2}$, hence, the operator in (150) can be expressed as

[TABLE]

The result in (158) can be written as

[TABLE]

According to (9) and (10), we have $\left\|\tilde{R}\right\|_{I}=\left\|\tilde{\rho}\right\|^{2}/\left(1+\left\|\tilde{\rho}\right\|^{2}\right)$ and $\boldsymbol{\Upsilon}_{a}\left(\tilde{R}\right)=2\tilde{\rho}/\left(1+\left\|\tilde{\rho}\right\|^{2}\right)$, while $\left\|\boldsymbol{\Upsilon}_{a}\left(\tilde{R}\right)\right\|^{2}=4\left(1-\left\|\tilde{R}\right\|_{I}\right)\left\|\tilde{R}\right\|_{I}=4\frac{\left\|\tilde{\rho}\right\|^{2}}{\left(1+\left\|\tilde{\rho}\right\|^{2}\right)^{2}}$ as in (11). Substituting for the differential operators $\dot{\hat{b}}$ and $\dot{\hat{\sigma}}$ and the correction factor $W$ from (92), (97) and (102), respectively, yields

[TABLE]

applying Young’s inequality to $k_{b}\tilde{b}^{\top}b$ and $k_{\sigma}\tilde{\sigma}^{\top}\sigma$, respectively, one has

[TABLE]

consequently, (172) becomes

[TABLE]

Setting $\gamma>0$, $\bar{\pi}>0$, $k_{p}k_{w}>4.5$, $k_{b}>0$, $k_{\sigma}>0$, and the positive constant $\varepsilon$ is sufficiently small, the operator $\mathcal{L}V$ in (172) becomes similar to (54) and (75) in [3] or (4.16) in [31] which is in turn similar to (50) in Lemma 1. In that case, the constant component $c_{2}$ in Lemma 1 is $c_{2}=\frac{k_{b}}{2}\left\|b\right\|^{2}+\frac{1}{2}\left(k_{\sigma}+\varepsilon\right)\left(\sum_{i=1}^{6}\sigma_{i}\right)^{2}$. Let us define

[TABLE]

The differential operator in (173) is

[TABLE]

and more simply

[TABLE]

such that $h\left(\cdot\right)$ is a class $\mathcal{K}$ function that includes the first two components in (174), and $\underline{\lambda}\left(\cdot\right)$ denotes the minimum eigenvalue of a matrix. Based on (175), one easily obtains

[TABLE]

as such, (176) means that

[TABLE]

The inequality in (177) implies that $\mathbb{E}\left[V\left(t\right)\right]$ is eventually bounded by $c_{2}/\underline{\lambda}\left(\mathcal{H}\right)$. Since, $\mathcal{Q}^{2}:\mathbb{R}_{+}\rightarrow\mathbb{R}^{6\times 6}$ is bounded, the operator in (176) is $\mathcal{L}V\leq c_{2}/\underline{\lambda}\left(\mathcal{H}\right)$. Define $\tilde{Z}=\left[\tilde{\rho}^{\top},\tilde{P}^{\top},\tilde{b}^{\top},\tilde{\sigma}^{\top}\right]^{\top}\in\mathbb{R}^{18}$, $\tilde{Z}$ is SGUUB in mean square as in Definition 1. Define $\mathcal{U}_{0}\subseteq\mathbb{SO}\left(3\right)\times\mathbb{R}^{3}\times\mathbb{R}^{6}\times\mathbb{R}^{6}$ by

[TABLE]

The set $\mathcal{U}_{0}$ is forward invariant and unstable. Therefore, from almost any initial condition such that $\tilde{R}\left(0\right)\notin\mathcal{U}_{0}$ or equivalently for any $\tilde{\rho}\left(0\right)\in\mathbb{R}^{3}$, the trajectory of $\tilde{Z}$ converges to the neighborhood of the origin which depends on the value of $c_{2}/\underline{\lambda}\left(\mathcal{H}\right)$ in (177). From Lemma 1 and design parameters of the stochastic observer in Theorem 2 in addition if we have prior knowledge about the covariance upper bound $\sigma$, $c_{2}/\underline{\lambda}\left(\mathcal{H}\right)$ can be made smaller if we choose the design parameters appropriately. Clearly, the minimum singular value of $\underline{\lambda}\left(\mathcal{H}\right)$ can be controlled by $k_{p}$, $k_{w}$, $\gamma$ and $\bar{\pi}$. To conclude our discussion, it should be remarked that solving the problem in the sense of Stratonovich with the proper selection of potential function as in (132) helps to attenuate or control the noise level associated with the velocity measurements vector $\mathcal{Y}_{m}$. The proposed nonlinear stochastic filter is able to correct the position as well as the attitude and reduce the noise level associated with velocity measurements $\mathcal{Y}_{m}$ through the setting of parameters in presence of high level of noise and bias components. This advantage is not given in nonlinear deterministic $\mathbb{SE}\left(3\right)$ filters. The main benefit of the nonlinear stochastic filter in the sense of Stratonovich is that no prior information about the covariance matrix $\mathcal{Q}^{2}$ is required. Also, the filter is applicable for white as well as colored noise which offers flexibility in the design process.

Remark 1

Notice that, as $k_{p},k_{w},\gamma,\bar{\pi}\rightarrow\infty$ and $\varepsilon\rightarrow 0$, $\mathbb{P}\left\{\underset{t\rightarrow\infty}{{\rm lim}}\left\|\tilde{X}\right\|=0\right\}\rightarrow 1,\forall t\geq 0$ with perfect cancellation of undesirable time-variant components and uncertainties.

5 Simulations

This section presents the performance of the proposed nonlinear stochastic filter on $\mathbb{SE}\left(3\right)$ considering high levels of bias and noise introduced in the measurement process combined with the large initial error in the homogeneous transformation matrix $\tilde{\boldsymbol{T}}\left(0\right)$. The performance of the proposed stochastic filter is compared to [17]. Let us define the dynamics of the homogeneous transformation matrix $\boldsymbol{T}$ as in (21). Let the angular velocity input signal be

[TABLE]

with initial attitude being $R\left(0\right)=\mathbf{I}_{3}$. Let the translational velocity be

[TABLE]

and the initial position $P\left(0\right)=\underline{\mathbf{0}}_{3}$. Let the angular velocity measurement $\Omega_{m}=\Omega+b_{\Omega}+\omega_{\Omega}$ be corrupted with a wide-band of random noise process with zero mean $\omega_{\Omega}$ and standard deviation (STD) equal to $0.15\left({\rm rad/sec}\right)$ and $b_{\Omega}=0.1\left[1,-1,1\right]^{\top}$. Similarly, let the translational velocity measurement $V_{m}=V+b_{V}+\omega_{V}$ be subject to a wide-band of random noise process $\omega_{V}$ with zero mean and ${\rm STD}=0.15\left({\rm m/sec}\right)$, and $b_{V}=0.1\left[2,5,1\right]^{\top}$.

Consider one landmark feature available for measurement $\left(N_{{\rm L}}=1\right)$

[TABLE]

and body-frame measurements obtained by (22) such that

[TABLE]

where the bias vector is defined as ${\rm b}_{1}^{\mathcal{B}\left({\rm L}\right)}=0.1\left[1.5,1,-1\right]^{\top}$ and a Gaussian noise vector $\omega_{1}^{\mathcal{B}\left({\rm L}\right)}$ with zero mean and ${\rm STD}=0.1$ corrupts the body-frame vector measurements associated with the feature point.

Consider that two non-collinear inertial-frame vectors $\left(N_{{\rm R}}=2\right)$ are given by

[TABLE]

while body-frame vectors ${\rm v}_{1}^{\mathcal{B}\left({\rm R}\right)}$ and ${\rm v}_{2}^{\mathcal{B}\left({\rm R}\right)}$ are obtained by (26)

[TABLE]

for $i=1,2$. The body-frame vector measurements are subject to bias components ${\rm b}_{1}^{\mathcal{B}\left({\rm R}\right)}=0.1\left[-1,1,0.5\right]^{\top}$ and ${\rm b}_{2}^{\mathcal{B}\left({\rm R}\right)}=0.1\left[0,0,1\right]^{\top}$. In addition to bias, Gaussian noise vectors $\omega_{1}^{\mathcal{B}\left({\rm R}\right)}$ and $\omega_{2}^{\mathcal{B}\left({\rm R}\right)}$ with zero mean and of ${\rm STD}=0.1$ corrupt the measurements. The third inertial and body-frame vector measurements are obtained by ${\rm v}_{3}^{\mathcal{I}\left({\rm R}\right)}={\rm v}_{1}^{\mathcal{I}\left({\rm R}\right)}\times{\rm v}_{2}^{\mathcal{I}\left({\rm R}\right)}$ and ${\rm v}_{3}^{\mathcal{B}\left({\rm R}\right)}={\rm v}_{1}^{\mathcal{B}\left({\rm R}\right)}\times{\rm v}_{2}^{\mathcal{B}\left({\rm R}\right)}$. Next, both body-frame and inertial-frame vectors are normalized, such that ${\rm v}_{i}^{\mathcal{B}\left({\rm R}\right)}$ and ${\rm v}_{i}^{\mathcal{I}\left({\rm R}\right)}$ are normalized to $\upsilon_{i}^{\mathcal{B}\left({\rm R}\right)}$ and $\upsilon_{i}^{\mathcal{I}\left({\rm R}\right)}$, respectively, for $i=1,2,3$ as given in (28). Therefore, Assumption 1 holds. From vectorial measurements, the corrupted reconstructed attitude $R_{y}$ is obtained by SVD [2] with $\tilde{R}=R_{y}\hat{R}^{\top}$, Appendix A. The total simulation time is 30 seconds.

For large initial attitude error, the initial rotation of attitude estimate is given according to the mapping of angle-axis parameterization in (7) by $\hat{R}\left(0\right)=\mathcal{R}_{\alpha}\left(\alpha,u/\left\|u\right\|\right)$ with $\alpha=170\left({\rm deg}\right)$ and $u=\left[3,10,8\right]^{\top}$ such that $\left\|\tilde{R}\left(0\right)\right\|_{I}$ approaches the unstable equilibria $+1$. Also, the initial position of the estimator is selected to be $\hat{P}\left(0\right)=\left[2,3,1\right]^{\top}$. The matrices below summarize the initial conditions:

[TABLE]

The initial estimates of $\hat{b}$ and $\hat{\sigma}$ are $\hat{b}\left(0\right)=\underline{\mathbf{0}}_{6}$ and $\hat{\sigma}\left(0\right)=\underline{\mathbf{0}}_{6}$. Design parameters used in the derivation of the nonlinear stochastic filter are selected as $\Gamma=\mathbf{I}_{6}$, $\Pi=\mathbf{I}_{6}$, $k_{b}=0.1$, $k_{\sigma}=0.1$, $k_{p}=2$, $k_{w}=3$, and $\varepsilon=0.5$. Additionally, the following color notation is used: green color refers to the true value, blue represents the performance of the proposed nonlinear stochastic filter, and red illustrates the performance of the filter previously proposed in literature. Finally, magenta demonstrates measured values.

The first three figures present the true values of the velocity vectors and body-frame vectors plotted against their measured values. The true angular velocity $\left(\Omega\right)$ and the high values of noise and bias components introduced through the measurement process of $\Omega_{m}$ plotted against time are depicted in Figure 2. Similarly, the true translational velocity $\left(V\right)$ and the high values of noise and bias components associated with the measurement process of $V_{m}$ plotted against time are illustrated in Figure 3. In addition, Figure 4 presents the true body-frame vectors and their uncertain measurements corrupted with noise. High levels of noise and bias inherent to the measurements can be noticed in all the above-mentioned graphs (Figure 2, 3 and 4).

The position and attitude tracking performance of the proposed stochastic filter is demonstrated in Figure 5 and 6. Figure 5 depicts the estimated Euler angles $\left({\rm\text{Roll}}\left(\hat{\phi}\right),{\rm\text{Pitch}}\left(\hat{\theta}\right),{\rm\text{Yaw}}\left(\hat{\psi}\right)\right)$ versus the true values $\left(\phi,\theta,\psi\right)$. Also, Figure 6 illustrates the high value of the attitude initial error. The tracking position $\left(\hat{x},\hat{y},\hat{z}\right)$ of the stochastic estimator in 3D space is compared to the true position $\left(x,y,z\right)$ over time in Figure 6. Figure 5 and 6 show impressive tracking performance of the proposed stochastic observer in terms of position and attitude in presence of large initial error between the true and the estimated pose. Also, Figure 5 and 6 demonstrate remarkable tracking performance in case when high values of bias and noise corrupt the measurements.

A comparison between the proposed stochastic observer in Theorem 2 and the deterministic pose observer in [17] is presented in Figure 7. The upper portion of Figure 7 illustrates the normalized Euclidean distance $\left\|\tilde{R}\right\|_{I}$, while the lower portion presents the Euclidean distance $\left\|P-\hat{P}\right\|$ for both observers such that $\tilde{R}=\hat{R}R^{\top}$. Figure 7 shows stable output performance of the stochastic observer with $\left\|\tilde{R}\right\|_{I}$ and $\left\|P-\hat{P}\right\|$ being regulated very close to the neighborhood of the origin confirming the results shown in Figure 5 and 6. On the other side, the deterministic filter shows high oscillatory performance before it goes out of stability.

Let $\mathring{{\rm v}}_{1}^{\mathcal{B}\left({\rm L}\right)}=R^{\top}\left({\rm v}_{1}^{\mathcal{I}\left({\rm L}\right)}-P\right)$ and $\mathring{{\rm v}}_{i}^{\mathcal{B}\left({\rm R}\right)}=R^{\top}{\rm v}_{i}^{\mathcal{I}\left({\rm R}\right)}$ denote the true body-frame vectors for $i=1,2,3$. Consider the error between the true and measured body-frame vectors $\tilde{{\rm v}}_{1}^{\mathcal{B}\left({\rm L}\right)}={\rm v}_{1}^{\mathcal{B}\left({\rm L}\right)}-\mathring{{\rm v}}_{1}^{\mathcal{B}\left({\rm L}\right)}$ and $\tilde{{\rm v}}_{i}^{\mathcal{B}\left({\rm R}\right)}={\rm v}_{i}^{\mathcal{B}\left({\rm R}\right)}-\mathring{{\rm v}}_{i}^{\mathcal{B}\left({\rm R}\right)}$. In the same spirit, let the error between the true and measured velocities be given by $\tilde{\Omega}=\Omega_{m}-\Omega$ and $\tilde{V}=V_{m}-V$. Table 1 provides mean and STD of the input measurements and the output data. It should be stressed that the mean errors of $\left\|\tilde{R}\right\|_{I}$ and $P-\hat{P}$ approach zero while the STD of $\left\|\tilde{R}\right\|_{I}$ is less than its mean, and the STD of $P-\hat{P}\approx 0.1$. Numerical results outlined in Table 1 affirm the robustness of the proposed nonlinear stochastic filter as demonstrated in Figure 5, 6, and 7.

Simulations presented in this section demonstrate the robustness of the proposed stochastic filter in the sense of Stratonovich against high levels of bias and noise components introduced in angular velocity, translational velocity and vectorial measurements. Also, they show that the stochastic filter is capable of correcting its position and attitude even in presence of large initial error in a small amount of time. In addition, the stochastic filter is autonomous, and therefore no prior information about the upper bound of the covariance matrix $\mathcal{Q}^{2}$ is required to achieve impressive estimation performance.

6 Conclusion

Pose is naturally nonlinear and is modeled on the Special Euclidean Group $\mathbb{SE}\left(3\right)$. Pose estimators used to be designed as nonlinear deterministic filters neglecting the noise inherent to the model dynamics. This is reflected in the nonlinear deterministic filter design as well as in the potential function selection. In this work, the pose problem has been formulated as a nonlinear pose problem on $\mathbb{SE}\left(3\right)$. The problem is mapped from $\mathbb{SE}\left(3\right)$ to vector form using Rodriguez vector parameterization and position. The problem is defined stochastically in the sense of Stratonovich. Next, a nonlinear stochastic pose filter on $\mathbb{SE}\left(3\right)$ has been proposed. It has been shown that errors in position, Rodriguez vector and estimates are semi-globally uniformly ultimately bounded (SGUUB) in mean square and that they converge to the small neighborhood of the origin for the case when noise is attached to the pose dynamics. Simulation results prove fast convergence from large initialized pose error even when angular and translational velocity vectors as well as body-frame measurements are subject to high levels of noise and bias.

Acknowledgment

The authors would like to thank University of Western Ontario for the funding that made this research possible. Also, the authors would like to thank Maria Shaposhnikova for proofreading the article.

Appendix A

An Overview on SVD in [2]

Let $R\in\mathbb{SO}\left(3\right)$ be the true attitude. The attitude can be reconstructed through a set of vectors given in (26). Let $s_{i}$ be the confidence level of measurement $i$ such that for $n$ measurements we have $\sum_{i=1}^{n}s_{i}=1$. In that case, the corrupted reconstructed attitude $R_{y}$ can be obtained by

[TABLE]

For more details visit [2].

AUTHOR INFORMATION

Hashim A. Hashim is a Ph.D. candidate and a Teaching and Research Assistant in Robotics and Control, Department of Electrical and Computer Engineering at the University of Western Ontario, ON, Canada.

His current research interests include stochastic and deterministic filters on SO(3) and SE(3), control of multi-agent systems, control applications and optimization techniques.

Contact Information: hmoham33@uwo.ca.

Lyndon J. Brown received the B.Sc. degree from the U. of Waterloo, Canada in 1988 and the M.Sc. and PhD. degrees from the University of Illinois, Urbana-Champaign in 1991 and 1996, respectively. He is an associate professor in the department of electrical and computer engineering at Western University, Canada. He worked in industry for Honeywell Aerospace Canada and E.I. DuPont de Nemours.

His current research includes the identification and control of predictable signals, biological control systems, welding control systems and attitude estimation.

Kenneth McIsaac received the B.Sc. degree from the University of Waterloo, Canada, in 1996, and the M.Sc. and Ph.D. degrees from the University of Pennsylvania, in 1998 and 2001, respectively. He is currently an Associate Professor and the Chair of Electrical and Computer Engineering with Western University, ON, Canada.

His current research interests include computer vision and signal processing, mostly in the context of using machine intelligence in robotics and assistive systems.