Unscented Multi-Point Smoother for Fusion of Delayed Displacement Measurements: Application to Agricultural Robots

Abstract

Visual Odometry (VO) is increasingly a useful tool for robotic navigation in a variety of applications, including weed removal for agricultural robotics. The methods of evaluating VO are often computationally expensive and can cause the VO measurements to be significantly delayed with respect to a compass, wheel odometry, and GPS measurements. In this paper we present a Bayesian formulation of fusing delayed displacement measurements. We implement solutions to this problem based on the unscented Kalman filter (UKF), leading to what we term an unscented multi-point smoother. The proposed methods are tested in simulations of an agricultural robot. The simulations show improvements in the localization RMS error when including the VO measurements with a variety of latencies.