Simultaneous localization and map building using constrained state estimate algorithm

Intelligent vehicles and autonomous robots are viable in complex environments, the reliable and robust localization function of which is necessary. Due to the large variability and uncertainty of complex environments, it is difficult to rely on a specific method or a set of sensor data to correctly and robustly estimate the robot pose. The key to solving the localization problem is to optimally use and fuse all useful sources of information available to the mobile platform. It is common to have approximate digital maps of the road network. A framework for simultaneous localization and map building (SLAM) problems using road constrained Kalman filter algorithms is developed, with the emphasis on vehicle applications in large environments. It presents the problems of outdoor navigation in areas with combination of features and on road regions. Road aided SLAM algorithms, which incorporate absolute information in a consistent manner, are presented. Kalman filters are commonly used to estimate the states of a mobile vehicle. However, in the application of Kalman filters, the known model or signal information often are either ignored or dealt with heuristically. For instance, constraints on state values which may be based on physical considerations are often neglected because they do not fit easily into the structure of the Kalman filter. This paper develops a rigorous analytic method of incorporating state equality constraints in the Kalman filter. The constraints may be time-varying, but it significantly improves the prediction accuracy of the filter. The SLAM implementation uses the road constrained Kalman filter algorithm to maintain the error of vehicle's location and mapping. Finally, the use of this algorithm is demonstrated on a simple vehicle tracking problem.