This work proposes a nonlinear stochastic filter evolved on the Special Orthogonal Group SO(3) as a solution to the attitude filtering problem. One of the most common potential functions for nonlinear deterministic attitude observers is studied and reformulated to address the noise attached to the attitude dynamics. The resultant estimator and correction factor demonstrate convergence properties and remarkable ability to attenuate the noise. The stochastic dynamics of the attitude problem are mapped from SO(3) to Rodriguez vector. The proposed stochastic filter evolved on SO(3) guarantees that errors in the Rodriguez vector and estimates steer very close to the neighborhood of the origin and that the errors are semi-globally uniformly ultimately bounded in mean square. Simulation results illustrate the robustness of the proposed filter in the presence of high uncertainties in measurements. Keywords: Attitude, estimate, estimator, observer, filter, stochastic differential equations, SDEs, Ito, Rodriguez vector, special orthogonal group, Euler angles, Brownian motion process, Angle-axis, Mapping, Parameterization, Representation, Partial derivative, asymptotic, unknown, time-varying, global, stable, stability, uncertain, white noise, Gaussian, colored, bias, vectorial, vector measurement, angular velocity, rotational matrix, identity, orientation, body frame, inertial frame, rigid body, three dimensional, micro electromechanical systems, Gyroscope, sensor, Inertial measurement units, IMUs, MEMS, Roll, Pitch, Yaw, autonomous, Robotic System, Spacecraft, submarine, Vehicles, Robot, Underwater vehicle, derivative, explicit complementary filter.
This paper presents theory, application, and comparisons of the feedback particle filter (FPF) algorithm for the problem of attitude estimation. The paper builds upon our recent work on the exact FPF solution of the continuous-time nonlinear filtering problem on compact Lie groups. In this paper, the details of the FPF algorithm are presented for the problem of attitude estimation - a nonlinear filtering problem on SO(3). The quaternions are employed for computational purposes. The algorithm requires a numerical solution of the filter gain function, and two methods are applied for this purpose…
This paper proposes two novel nonlinear attitude filters evolved directly on the Special Orthogonal Group SO(3) able to ensure prescribed measures of transient and steady-state performance. The tracking performance of the normalized Euclidean distance of attitude error is trapped to initially start within a large set and converge systematically and asymptotically to the origin from almost any initial condition. The convergence rate is guaranteed to be less than the prescribed value and the steady-state error does not exceed a predefined small value. The first filter uses a set of vectorial mea…
We revisit the nonlinear complimentary filter on $SO(3)$, previously proposed in the literature, and provide the (time-explicit) solution to the matrix ODE governing the attitude estimation error in the absence of measurement errors. The stability and performance properties of this filter can be easily deduced from the obtained closed-from solution. Thereafter, we consider two nonlinear complimentary filters (with state-dependent gains) which are shown to exhibit improved stability and performance proprieties compared to the traditional filter. We perform robustness analysis for the three disc…
We revisit the gradient based nonlinear attitude complementary filters (observers) on the Special Orthogonal group SO(3) and provide explicit solutions of the norm of the attitude estimation error dynamics. One smooth and two non-smooth attitude observers are considered. The stability and performance properties of theses attitude observers can be easily deduced from the obtained explicit solutions. We show that the smooth complementary attitude filter, previously proposed in the literature, is not Input-to-State-Stable (ISS) with respect to bounded measurement disturbances while the non-smooth…
This paper proposes an integrated design optimization framework for the gain schedules and bending filter for the longitudinal control of a rocket during its ascent flight. Dynamic models representing the pitch/yaw motion of a rocket considering the elements such as the rigid body dynamics, aerodynamics, sloshing, bending, sensor/actuator, and flight computer are introduced. The linear proportional and differential (PD) control law with scheduled (time-varying) gains and bending filter parameters are identified as key decision variables for stabilizing the pitch/yaw motion of the rocket. The i…
This paper focuses on a stochastic formulation of Bayesian attitude estimation on the special orthogonal group. In particular, an exponential probability density model for random matrices, referred to as the matrix Fisher distribution is used to represent the uncertainties of attitude estimates and measurements in a global fashion. Various stochastic properties of the matrix Fisher distribution are derived on the special orthogonal group, and based on these, two types of intrinsic frameworks for Bayesian attitude estimation are constructed. These avoid complexities or singularities of the atti…
Attitude estimation for small, low-cost unmanned aerial vehicles is often achieved using a relatively simple complementary filter that combines onboard accelerometers, gyroscopes, and magnetometer sensing. This paper explores the limits of performance of such attitude estimation, with a focus on performance in highly dynamic maneuvers. The complementary filter is derived along with the extended Kalman filter and unscented Kalman filter to evaluate the potential performance gains when using a more sophisticated estimator. Simulations are presented that compare performance across a range of test…
The attitude of a rigid body evolves on the three-dimensional special orthogonal group, and it is often estimated by measuring reference directions, such as gravity or magnetic field, using an onboard sensor. As a single direction measurement provides a two-dimensional constraint, it has been widely accepted that at least two non-parallel reference directions should be measured, or the reference direction should change over time, to determine the attitude completely. This paper uncovers an intriguing fact that the attitude can actually be estimated by using multiple measurements of a single, f…
Estimation of rigid body attitude motion is a long-standing problem of interest in several applications. This problem is challenging primarily because rigid body motion is described by nonlinear dynamics and the state space is nonlinear. The extended Kalman filter and its several variants have remained the standard and most commonly used schemes for attitude estimation over the last several decades. These schemes are obtained as approximate solutions to the nonlinear optimal filtering problem. However, these approximate or near optimal solutions may not give stable estimation schemes in genera…
Among algorithms used for sensor fusion for attitude estimation in unmanned aerial vehicles, the Extended Kalman Filter (EKF) is the most commonly used for estimation. In this paper, we propose a new version of H2 estimation called extended H2 estimation that can overcome the limitations of the extended Kalman Filter, specifically with respect to computational speed, memory usage, and root mean squared error. We formulate a new attitude-estimation algorithm, where the filter gain is designed offline about a nominal operating point, but the filter dynamics is implemented using the nonlinear sys…
Motivated by drone autonomous navigation applications we address a novel problem of velocity-aided attitude estimation by combining two linear velocity components measured in a body-fixed frame and a linear velocity component measured in an inertial frame with the measurements of an Inertial Measurement Unit (IMU). The main contributions of the present paper are the design of Riccati nonlinear observers, which may be viewed as deterministic versions of an Extended Kalman filter (EKF), and an analysis of observability conditions under which local exponential stability of the observer is achieve…
This paper is concerned with the problem of continuous-time nonlinear filtering for stochastic processes on a connected matrix Lie group. The main contribution of this paper is to derive the feedback particle filter (FPF) algorithm for this problem. In its general form, the FPF is shown to provide a coordinate-free description of the filter that automatically satisfies the geometric constraints of the manifold. The particle dynamics are encapsulated in a Stratonovich stochastic differential equation that retains the feedback structure of the original (Euclidean) FPF. The implementation of the …
Two novel nonlinear pose (i.e, attitude and position) filters developed directly on the Special Euclidean Group SE(3)able to guarantee prescribed characteristics of transient and steady-state performance are proposed. The position error and normalized Euclidean distance of attitude error are trapped to arbitrarily start within a given large set and converge systematically and asymptotically to the origin from almost any initial condition. The transient error is guaranteed not to exceed a prescribed value while the steady-state error is bounded by a predefined small value. The first pose filter…
Closed-loop attitude steering can be used to implement a non-standard attitude maneuver by using a conventional attitude control system to track a non-standard attitude profile. The idea has been employed to perform zero-propellant maneuvers on the International Space Station and minimum time maneuvers on NASA's TRACE space telescope. A challenge for operational implementation of the idea is the finite capacity of a space vehicle's command storage buffer. One approach to mitigate the problem is to downsample-and-hold the attitude commands as a set of waypoints for the attitude control system t…
This paper presents a novel nonlinear pose filter evolved directly on the Special Euclidean Group SE(3) with guaranteed characteristics of transient and steady-state performance. The above-mention characteristics can be achieved by trapping the position error and the error of the normalized Euclidean distance of the attitude in a given large set and guiding them to converge systematically to a small given set. The error vector is proven to approach the origin asymptotically from almost any initial condition. The proposed filter is able to provide a reliable pose estimate with remarkable conver…
This paper mainly discusses the $H_{\infty}$ filtering of general nonlinear discrete time-varying stochastic systems. A nonlinear discrete-time stochastic bounded real lemma (SBRL) is firstly obtained by means of the smoothness of the conditional mathematical expectation, and then, based on the given SBRL and a stochastic LaSalle-type theorem, a sufficient condition for the existence of the $H_\infty$ filtering of general nonlinear discrete time-varying stochastic systems is presented via a new introduced Hamilton-Jacobi inequality (HJI), which is easily verified. When the worst-case disturban…
This paper studies the mixed $H_-/H_{\infty}$ fault detection filtering of Itô-type nonlinear stochastic systems. Mixed $H_-/H_{\infty}$ filtering combines the system robustness to the external disturbance and the sensitivity to the fault of the residual signal. Firstly, for Itô-type affine nonlinear stochastic systems, some sufficient criteria are obtained for the existence of $H_-/H_{\infty}$ filter in terms of Hamilton-Jacobi inequalities (HJIs). Secondly, for a class of quasi-linear Itô systems, a sufficient condition is given for the existence of $H_-/H_{\infty}$ filter by means of linear…
In this paper we introduce a new approach to the study of filtering theory by allowing the system's parameters to have a random character. We use Hida's white noise space theory to give an alternative characterization and a proper generalization to the Wiener filter over a suitable space of stochastic distributions introduced by Kondratiev. The main idea throughout this paper is to use the nuclearity of this spaces in order to view the random variables as bounded multiplication operators (with respect to the Wick product) between Hilbert spaces of stochastic distributions. This allows us to us…
Despite being a foundational concept of modern systems theory, there have been few studies on observability of non-linear stochastic systems under partial observations. In this paper, we introduce a definition of observability for stochastic non-linear dynamical systems which involves an explicit functional characterization. To justify its operational use, we establish that this definition implies filter stability under mild continuity conditions: an incorrectly initialized non-linear filter is said to be stable if the filter eventually corrects itself with the arrival of new measurement infor…
We consider the problem of estimating the dynamic latent states of an intracellular multiscale stochastic reaction network from time-course measurements of fluorescent reporters. We first prove that accurate solutions to the filtering problem can be constructed by solving the filtering problem for a reduced model that represents the dynamics as a hybrid process. The model reduction is based on exploiting the time-scale separations in the original network, and it can greatly reduce the computational effort required to simulate the dynamics. This enables us to develop efficient particle filters …
