Optimal position and velocity navigation filters for autonomous vehicles

Abstract

This paper presents the design and performance evaluation of a set of globally asymptotically stable time-varying kinematic filters with application to the estimation of linear motion quantities of mobile platforms (position, linear velocity, and acceleration) in three dimensions. The proposed techniques are based on the Kalman and ${\mathcal{H}}_{\infty }$ optimal filters for linear time-varying systems and the explicit optimal filtering solutions are obtained through the use of an appropriate coordinate transformation, whereas the design employs frequency weights to achieve adequate disturbance rejection and attenuation of the measurement noise on the state estimates. Two examples of application in the field of ocean robotics are presented that demonstrate the potential and usefulness of the proposed design methodology. In the first the proposed filtering solutions allow for the design of a complementary navigation filter for the estimation of unknown constant ocean currents, while the second addresses the problem of estimation of the velocity of an underwater vehicle, as well as the acceleration of gravity. Simulation results are included that illustrate the filtering achievable performance in the presence of both extreme environmental disturbances and realistic measurement noise.

Introduction

Navigation and Positioning Systems play a key role in the development of a large variety of mobile platforms for land, air, space, and marine applications. In the domain of marine research, for instance, the quality of the navigation data is a fundamental requirement in applications that range from ocean sonar surveying to ocean data acquisition or sample collection, as the acquired data sets should be properly geo-referenced with respect to a given mission reference point. For control purposes other quantities such as the attitude of the vehicle and/or the linear and angular velocities are also commonly required. This paper presents a set of optimal time-varying filtering solutions for a class of kinematic systems with direct application to the estimation of linear motion quantities in accurate Integrated Navigation Systems.

To tackle this class of problems several approaches have been proposed in the literature. In Fossen and Grøvlen (1998) a globally exponentially stable (GES) nonlinear control law is presented for ships, in two dimensions, which includes a nonlinear observer to provide the state of the vehicle. This observer relies on the vehicle dynamics but, as discussed in Robertsson and Johansson (1998), it does not apply to unstable ships. In the latter a solution to an extended class of ships is proposed requiring only stable surge dynamics. In Fossen and Strand (1999) a GES observer for ships (in two dimensions) that includes features such as wave filtering and bias estimation is presented and in Nijmeijer and Fossen (1999) an extension to this result with adaptive wave filtering is available. An alternative filter was proposed in Pascoal, Kaminer, and Oliveira (2000) where the problem of estimating the velocity and position of an autonomous vehicle in three dimensions was solved by resorting to special bilinear time-varying complementary filters. More recently, a pair of coworking nonlinear Luenberger GES observers for autonomous underwater vehicles (AUVs), in 3D, was proposed in Refsnes, Sorensen, and Pettersen (2006), which also elaborates on the destabilizing Coriolis and centripetal forces and moments. However, this last approach assumes, among others, limited pitch angles. A more complete survey on the subject of underwater vehicle navigation can be found in Kinsey, Eustice, and Whitcomb (2006). General drawbacks of the above-mentioned results include the absence of systematic tuning procedures and the inherent limitations of the vehicle dynamic models, which are seldom known in full detail and may be subject to variations over time. Previous work by the authors can be found in Batista, Silvestre, and Oliveira (2008).

The main contribution of this paper is a new filtering design methodology for a class of kinematic systems with application to the estimation of linear quantities (position, linear velocity, ocean current, and gravity acceleration) in Integrated Navigation Systems that

• presents globally exponentially stable error dynamics which are also globally input-to-state stable (ISS) with respect to disturbances on the key quantities;

• is optimal with respect to disturbances arising from all sensors but the Attitude and Heading Reference System (AHRS);

• provides a systematic design procedure based upon well known filtering results, that allows for the use of frequency weights to shape the dynamic response of the filter;

• includes a limit filtering solution that is computationally efficient and therefore appropriate for implementation in low-cost, low-power hardware.

The methodology proposed in this paper relies on the design of optimal time-varying Navigation filters based on the steady-state Kalman and ${\mathcal{H}}_{\infty }$ filter solutions for equivalent linear time invariant (LTI) systems. This equivalence is established through a time-varying orthogonal coordinate transformation that is readily available from an AHRS. An example of previous use of Lyapunov transformations in the design of estimators can be found in Nguyen and Lee (1985). Applications of the proposed filter design techniques are presented to estimate linear quantities in Integrated Navigation Systems for mobile platforms. To describe the vehicle tri-dimensional motion the proposed filters rely on pure kinematic models. This class of models, expressed in the inertial coordinate system, has been widely used by the Navigation community, see Savage (1998) and references therein. The present solution departs from previous approaches as it considers the rigid-body kinematics expressed in body-fixed coordinates, thus avoiding the algebraic transformation of sensor data to inertial coordinates, which is strongly undesirable since the effect of the noise on the attitude may be greatly amplified on the state estimates.

The paper is organized as follows. The motivation behind the work is presented in Section 2, whereas the theoretical background that supports the navigation solutions proposed in the paper is presented in Section 3. Simulation results are included, in Section 4, that illustrate the achievable performance of the proposed solutions in the presence of extreme environmental disturbances and realistic measurement noise. Finally, Section 5 summarizes the main contributions of the paper.

Notation and definitions: Throughout the paper the symbol $0_{n\times m}$ denotes an $n\times m$ matrix of zeros, $I_n$ an identity matrix with dimension $n\times n$, and $\mathrm{diag}(A_1,\dots ,A_n)$ a block diagonal matrix. When omitted the matrices are assumed of appropriate dimensions. The Special Orthogonal Group $\{R\in {\mathbb{R}}^{3\times 3}:R^{T}R=I_3,det\left(R\right)=1\}$ is denoted by $SO\left(3\right)$ and the usual Hilbert space of square integrable functions is denoted by ${\mathcal{L}}_2$.