Abstract. This work addresses real time implementation of the Simultaneous Localization and Map Building (SLAM) algorithm. It presents optimal algorithms that consider the special form of the matrices and a new compressed filter that can significantly reduce the computation requirements when working in local areas or with high frequency external sensors. It is shown that by extending the standard Kalman filter models the information gained in a local area can be maintained with a cost O(N a 2 ), where N a is the number of landmarks in the local area, and then transferred to the overall map in only one iteration at full SLAM computational cost. Additional simplifications are also presented that are very close to optimal when an appropriate map representation is used. Finally the algorithms are validated with experimental results obtained with a standard vehicle running in a completely unstructured outdoor environment.

Abstract-This paper presents an analysis of the extended Kalman filter formulation of simultaneous localisation and mapping (EKF-SLAM). We show that the algorithm produces very optimistic estimates once the "true" uncertainty in vehicle heading exceeds a limit. This failure is subtle and cannot, in general, be detected without ground-truth, although a very inconsistent filter may exhibit observable symptoms, such as disproportionately large jumps in the vehicle pose update. Conventional solutions-adding stabilising noise, using an iterated EKF or unscented filter, etc-do not improve the situation. However, if "small" heading uncertainty is maintained, EKF-SLAM exhibits consistent behaviour over an extended time-period. Although the uncertainty estimate slowly becomes optimistic, inconsistency can be mitigated indefinitely by applying tactics such as batch updates or stabilising noise. The manageable degradation of small heading variance SLAM indicates the efficacy of submap methods for large-scale maps.

This paper presents the design of a high accuracy outdoor navigation system based on standard dead reckoning sensors and laser range and bearing information. The data validation problem is addressed using laser intensity information. The beacon design aspect and location of landmarks are also discussed in relation to desired accuracy and required area of operation.The results are important for Simultaneous Localization and Map building applications, (SLAM), since the feature extraction and validation are resolved at the sensor level using laser intensity. This facilitates the use of additional natural landmarks to improve the accuracy of the localization algorithm. The modelling aspects to implement SLAM with beacons and natural features are also presented. These results are of fundamental importance because the implementation of the algorithm does not require the surveying of beacons. Furthermore we demonstrate that by using natural landmarks high accurate localization can be achieved by only requiring the initial estimate of the position of the vehicle. The algorithms are validated in outdoor environments using a standard utility car retrofitted with the navigation sensors and a 1 cm precision Kinematic GPS used as ground truth.