Position and Velocity Optimal Sensor-based Navigation Filters for UAVs Pedro Batista, Carlos Silvestre, and Paulo Oliveira Abstract— The paper presents the design and performance evaluation of a novel navigation solution that merges low-rate delayed GPS measurements with high-rate linear acceleration, attitude, and angular velocity measurements to estimate, in three dimensions, linear motion quantities (position, linear velocity, an acceleration of gravity) of unmanned aerial vehicles (UAVs). The design is based on the continuous-discrete Kalman filter solution for an equivalent LTI realization and allows for the natural use of frequency weights to explicitly achieve ade- quate disturbance rejection and measurement noise attenuation on the state estimates. The proposed solution is optimal with respect to all quantities assuming exact angular measurements and, in the presence of noisy angular quantities, it outperforms classic solutions developed in inertial coordinates. Simulation results illustrate the achievable performance in the presence of realistic measurements, including noise and delays. I. I NTRODUCTION The design of Navigation and Positioning Systems plays a key role in the development of UAVs. On one hand, the acquired data sets should be properly georeferenced with respect to a given mission reference point on survey tasks. On the other hand, good navigation information is necessary for control purposes, where other quantities such as the attitude of the vehicle and the (linear and angular) velocities are also usually required. This paper presents a novel navigation solu- tion to estimate linear motion quantities, in three dimensions, with application to UAVs. Inertial Navigation Systems (INS) have been extensively studied in the past, as evidenced by the large number of publications on the subject, see [1], [2], [3], [4], [5], and references therein. These systems have very high short-term accuracy but, since they integrate, in open-loop, noisy quanti- ties, the performance gets degraded over time. This limitation is successfully overcome resorting to aiding sensors such as the Global Positioning System (GPS) and several solutions have been proposed in the literature [6], [7]. The main contribution of the paper is the design and performance evaluation of a novel navigation solution that merges low-rate position GPS measurements with high-rate linear acceleration, attitude, and angular velocity measure- ments to provide continuous-time estimates of the linear This work was partially supported by Fundac ¸˜ ao para a Ciˆ encia e a Tecnologia (ISR/IST plurianual funding) through the POS Conheci- mento Program that includes FEDER funds and by the PTDC/EEA- ACR/72853/2006 OBSERVFLY Project. The work of P. Batista was sup- ported by a PhD Student Scholarship from the POCTI Programme of FCT, SFRH/BD/24862/2005. The authors are with the Institute for Systems and Robotics, Insti- tuto Superior T´ ecnico, Av. Rovisco Pais, 1049-001 Lisboa, Portugal. {pbatista,cjs,pjcro}@isr.ist.utl.pt motion quantities (position, velocity, and acceleration of gravity) in three dimensions. The present solution departs from previous approaches as it considers each variable in the most natural space, i.e., the space where it is measured, thus avoiding the rotation of the acceleration to inertial coordinates and the correction of the gravity and Coriolis acceleration terms. In fact, the gravity is also estimated, in body-fixed coordinates, and the Coriolis acceleration is explicitly taken into account in the model. These last two points are of major importance in the design of Navigation Systems as, due to its magnitude, any misalignment in the estimate of the acceleration of gravity or the Coriolis acceleration results in severe problems in the acceleration compensation. The advantages of the proposed solution are also evident in the case of vehicle stabilization and control. Indeed, while in classic estimation solutions the velocity of the vehicle is estimated in inertial coordinates and must be converted to body-fixed coordinates, with the proposed solution the velocity is directly estimated in body-fixed coordinates, thus reducing the impact of the noise of the attitude measurements. The same applies to the acceleration of gravity. At the core of the proposed methodology there is a time- varying orthogonal Lyapunov transformation that renders the dynamics of the kinematic system linear time invariant (LTI). This allows for the derivation of an equivalent continuous- time Kalman filter with discrete-time delayed measurements for the LTI realization, which is then converted back to the original time-varying framework, yielding the final optimal filtering solution. Frequency weights may be included in the design to explicitly achieve adequate disturbance rejection and measurement noise attenuation on the state estimates and the proposed solution is optimal with respect to all signals assuming exact angular measurements. Interestingly enough, it is precisely in the presence of noisy angular quantities that the present solution exhibits more clearly its advantages over traditional approaches, as it will be shown. Finally, an explicit limit filtering solution is presented which does not require the solution of a Lyapunov matrix differential equation each time a new measurement arrives. This is of great practical importance since it lessens the computational cost and allows for a straightforward digital implementation of the filter. The paper is organized as follows. The estimation problem and the system dynamics are introduced in Section II. The filter design is derived in Section III, where a limit filter- ing solution and alternative approaches are also discussed. 2009 American Control Conference Hyatt Regency Riverfront, St. Louis, MO, USA June 10-12, 2009 FrC07.6 978-1-4244-4524-0/09/$25.00 ©2009 AACC 5404