Automatica 46 (2010) 767–774 Contents lists available at ScienceDirect Automatica journal homepage: www.elsevier.com/locate/automatica Brief paper Optimal position and velocity navigation filters for autonomous vehicles ✩ Pedro Batista ∗ , Carlos Silvestre, Paulo Oliveira Instituto Superior Técnico, Institute for Systems and Robotics, Av. Rovisco Pais, 1049-001 Lisboa, Portugal article info Article history: Received 10 October 2008 Received in revised form 22 July 2009 Accepted 22 January 2010 Available online 4 March 2010 Keywords: Underwater vehicles Navigation Mobile robots Autonomous systems Estimation theory 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 H ∞ 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. © 2010 Elsevier Ltd. All rights reserved. 1. 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 ✩ This work was partially supported by Fundação para a Ciência e a Tecnologia (ISR/IST plurianual funding), by the projects PDCT/MAR/55609/2004-RUMOS and PTDC/MAR/64546/2006-OBSERVFLY of the FCT, and by the EU Project TRIDENT (Contract No. 248497). The material in this paper was partially presented at the 17th IFAC World Congress, July 2008, South Korea. This paper was recommended for publication in revised form by Associate Editor Masaki Yamakita under the direction of Editor Toshiharu Sugie. The work of P. Batista was supported by a Ph.D. Student Scholarship from the POCTI Programme of FCT, SFRH/BD/24862/2005. ∗ Corresponding author. Tel.: +351 218418054; fax: +351 218418291. E-mail address: pbatista@isr.ist.utl.pt (P. Batista). 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 0005-1098/$ – see front matter © 2010 Elsevier Ltd. All rights reserved. doi:10.1016/j.automatica.2010.02.004