Optimal position and velocity navigation filters for autonomous vehicles

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