NONLINEAR KALMAN BASED FILTERING FOR POSE ESTIMATION OF A ROBOTIC VEHICLE FROM DISCRETE ASYNCHRONOUS RANGE MEASUREMENTS

Abstract: We consider the problem of estimating the position and orientation of a robotic vehicle moving in a 2D plane using range measurements from the vehicle to fixed beacons that arrive synchronously/asynchronously at discrete instants of time. In this setup we assume that the vehicle is equipped with only one range measuring sensor and that the set composed by the measurements at each fixed time may not be complete. We analyze the observability of the system and in particular we show that there exist conditions under which the system is not observable. Two nonlinear Kalman based filtering algorithms are proposed: the extended Kalman filter (EKF) and the unscented Kalman filter (UKF). Simulation results are used to compare the performance of the filters. The results show that when the measurements arrive synchronously, both the filters exhibit good performance if the initial estimation errors are small. In this case we did not notice any major difference in the estimation accuracy between the two filters. When the range measurements are not received at the same time, the performance of the filters degrade as it is expected