Partially Constrained Extended Kalman Filter for Navigation Including Mapping Information

Near-optimal states estimation in dynamic systems with Gaussian noise has been extensively applied in research using Kalman filtering (KF). This method is especially useful in technological applications that use noisy data from several sensors or need the estimation of an optimal state vector in systems with multiple observations. A particular situation is that in which some constraints are applied to the state vector, for instance, assigning predetermined values to a part of the state vector or imposing a particular relationship between some variables of the state vector. This paper deals with the application of an extended KF (EKF) for mobile robot (MR) indoor navigation fusing the relative positioning obtained with the robot odometry with the absolute positioning measured with a set of ultrasonic local positioning systems. The novelty of the system lies on the use of a map description as additional information, in such a way that the estimation of the trajectory of MR does not cross walls or other physical instances at any time. If after a vector state estimation such a situation is detected, an analytical method is used to impose some constraints to a part of the state vector so the physical restrictions are considered. The proposal has been compared to a numerical method based on constrained optimization (using the function fmincon of MATLAB) or based on the use of a particle filter (PF) that is often used to solve this kind of problems. Simulated and real test has been carried out to show the effectiveness of the method, obtaining our approach a better performance in terms of accuracy compared with the traditional EKF at the expense of a minor increase in complexity, and obtaining a better performance compared with PF.