Invariant filtering for simultaneous localization and mapping

This paper presents an algorithm for simultaneous localization and map building for a mobile robot moving in an unknown environment. The robot can measure only the bearings to identifiable targets and its own relative motion. The approach is to recursively estimate features of the environment which are invariant to the robot pose in order to decouple the pose error from the map error. The highly nonlinear nature of this problem requires more explicit reasoning about the spatial relationships between landmarks and between the robot and landmarks than those used in previous methods.


Introduction
It may be required for a robot to enter an unknown environment and to concurrently explore and produce a metric map of the area while maintaining an accurate estimate of its position.If the robot were to have a n a priori map, then localization would be a relatively easy task.Alternatively, if the robot were to have a precise, externally referenced position estimate, then mapping would be an easy task.However, problems in which the robot has no a priori map and no external position reference are particularly challenging.Such scenarios may arrise for AUV's, mining applications, or planetary surfaces.This problem has been referred to as concurrent localization and mapping CLM and simultaneous localization and mapping SLAM.We will use the latter in this paper.Section 2 discusses some previous work in SLAM.Section 3 discusses the general ideas behind how the Invariant Filter works.Section 4 details the algorithm.Section 5 shows results from implementing the lter and testing it using real data.

Previous Work
The SLAM problem was originally taken on by Smith, Self and Cheeseman 1 who rst published the stochastic map, and by Moutarlier and Chatila 2 who provided experiments using this approach with real data.Because the stochastic mapping approach is based on the Extended Kalman Filter, the update requires an ON 3 matrix inversion, where N is the number of landmarks or environment states to be estimated.Approaches based on the stochastic map method are common and a good review can be found in 3 .Recently, h o w ever, new solutions to the SLAM problem have been reported which take a principled approach to decoupling the problem in order to keep the computation tractable.If the state covariance matrix is diagonal or block diagonal, inversion becomes simpler and the EKF update can be done much faster.Leonard and Feder have recently introduced the Decoupled Stochastic Mapping DSM algorithm 4 which tesselates the environment with loosely coupled local maps, each of which is a full stochastic map.Between local maps the relationship is approximate and covariances are not considered.The computational complexity of the stochastic map is incurred only within each local map.Csorba and Durrant-Whyte introduced the concept of a relative lter 5 , which recursively estimates features which are invariant to robot position, thereby removing the correlation between the robot position uncertainty and feature uncertainties, as well as feature-feature correlations.Newman expanded on the method, introducing the Geometric Projection Filter GPF 6 which provides a means by which to produce a geometrically consistent map from the relative features in Csorba's lter by solving a set of simultaneous linear constraint equations to project the relative map states onto an absolute coordinate system.To the authors' knowledge, previous implementations of the relative lter and GPF have assumed that absolute heading is measurable e.g. by a compass and that bearing and range to each target is measurable.Under these conditions, the relative lter or GPF can use environment features which are invariant only under translation and treat the problem in a linear fashion.In this paper we present an algorithm for Invariant Filtering, which is a more general version of the GPF 6 .The approach is based on the same principles as the relative lter and GPF, but is able to deal with unknown heading as well by ltering environment features which are invariant under rotation and translation, not just translation.We also consider the use of a bearings only sensor, using an omnidirectional camera to track landmarks as the robot moves.

Invariant Filter
Consider a mobile robot which can measure the bearings to landmarks, which w e will de ne to be uniquely identi able point features.The robot can measure its own motion through odometry or inertial sensing.We will restrict our discussion here to the planar case.The robot's pose in x; y; is parameterized by the vector x k .There is some nonlinear stochastic process model which, given an initial position estimate xk and a motion measurement u k , predicts the nal position of the robot xk+1 xk+1 = Fx k ; u k + w k 1 where w k is taken to be zero mean Gaussian noise with covariance Q.The robot is equipped with a bearing sensor which makes a noisy measurement of the bearing to a landmark from the robot's current pose, where k is zero mean Gaussian noise with covariance R. If the robot measures the bearing to a landmark One major di culty with absolute coordinate frames in SLAM is that landmark position estimates ^i and the robot pose estimate xk are correlated.Assuming for the moment that the bearing measurements z i k and z i k+1 are perfect, the error in the landmark position due to the error in robot pose can be found by expanding ~i then taking a Taylor expansion ~i = ^i , Dropping all but the rst order term, 1 shows a diagram of how this happens.The vehicle measures the bearing to a target, moves, and measures the bearing to the target again.The position of the target is measured in a rover-centered coordinate frame with the initial robot position as its origin.The resulting estimate of the landmark location in absolute coordinates depends on where the vehicle was when it began its motion and the error in landmark position is directly correlated with the error in robot position.
A similar argument demonstrates how the error in the robot pose estimate becomes correlated to errors in the map when the rover attempts to localize relative to landmarks with erroneous locations.Over time, ltering methods such as Kalman lters which recursively update robot pose and landmark positions in an absolute coordinate frame can diverge.In addition, absolute ltering approaches to solving the SLAM problem su er the curse of dimensionality.The state space is of dimension N where in a planar environment, N is three parameters for the robot pose plus two parameters for every landmark.Kalman lters require ON 2 storage and ON 3 computation per step in order to keep track of the full covariance matrix, and as the robot explores larger and larger regions, the lter becomes intractable.The fundamental importance of the relative lter 5 i s that it recursively updates estimates of environmental features which are invariant to the robot pose.Consider the distance between two landmarks, d ij = jj`i , `jjj 2 : 6 The estimate dij and error dij do not depend on x k .
If Dx k ; u k ; z i k ; z j k ; z i k +1 ; z j k +1 is the estimator for d ij , then r xk Dx k ; u k ; z i k ; z j k ; z i k +1 ; z j k +1 = 0 7 and therefore E xk dij k = 0 : 8 Even an error in heading, which is notoriously problematic for dead reckoning, does not a ect the estimate.Figure 2 shows a robot estimating the distance d ij between landmarks i and j.The position of each landmark is wrong due to xk but the relationship between the landmarks is una ected.The two i n v ariant features which can be estimated are pairwise distances, as mentioned above, and angles ijk subtended by a landmark pair `i; k as measured from a third, `j , as in Figure 3.The endpoints which de ne a distance, or the triplet of points which dene an angle, are called control points.F or simplicity, we will deal with only distance measurements for the rest of this paper, but note that angles may be more di cult to use.Angles represent more nonlinear relationships and may present singularities.In addition, estimating angles requires the simultaneous observation of three landmarks instead of two.

The Algorithm
In this section we will detail the the current implementation of the algorithm.The invariant lter requires two separate maps" to be maintained.The rst map is a relative map similar to that of 5 .This map contains the invariant relationships between landmarks.Each i n v ariant feature has an associated type distance or angle, a list of the landmarks which make up its control points, and a mean and covariance for the estimated feature value.
The second map is an absolute map which contains estimates for landmark positions in a global coordinate frame.Each landmark in the absolute map contains an estimated x; y coordinate and a list of the invariant features for which i t i s a c o n trol point.This enables e cient computation of the derivatives for optimizing the absolute map with respect to the relative map. Figure 4 shows a diagram of these data structures and their relationships.feature [1]   . . .

Building the maps
When a new landmark is encountered, its position is estimated in the global reference frame using the estimate of the robot pose and an estimate of the landmark position in the robot coordinate frame.The landmark is then inserted into the absolute map at that location.It is only important that the landmark be inserted close enough to its true" position that the optimization procedure used to update the absolute map converges to the right solution.Invariant features involving the new landmark must also be inserted into the relative map.A greedy triangulation algorithm is used to connect the new landmark to observable neighbors.Note that we do not require a planar triangulation.The triangulation must simply provide enough constraints to x the landmarks in the absolute map.An attempt is made to create invariant features using landmarks which are close together, since landmarks which are far apart are less likely to be observed simultaneously.

Strategy for avoiding correlations
The goal in this ltering method is to ensure that the state vector is decoupled so that memory requirements and update computation are trivial.In order to do this, we m ust be careful not to introduce correlations.Correlation between robot pose error and landmark position errors is eliminated by using map features which are invariant to robot pose.By proper consideration, we can also eliminate correlation between map features.When bearing observations are made, the bearings Z k = fz i k ; i = 0 ; : : : gto all visible targets at time k are stored.The measurement u k of motion from time k to k + 1 is also stored.Triangulation allows computation of the landmark positions using the motion estimate as a baseline, ^i = Lx k ; Fx k ; u k ; z i k ; z i k +1 : 9 We then compute the Euclidean distance between pairs of landmarks as dij = jj ^i , ^jjj 2 .The error dij will be correlated with u k , z i k , z i k+1 , z j k , and z j k+1 .In order to be sure that the estimate dij is not correlated with any other features in the relative map, we cannot use any o f f u k ; z i k ; z i k +1 ; z j k ; z j k +1 g to estimate other features.The restriction that we cannot use z i k+1 and z j k+1 means that we m ust remove those measurements from the bu er.The restriction that we cannot use u k means that not only will u k be removed from the bu er, but also all old bearing measurements Z k as well, since these bearings cannot be used for triangulation without estimates of motion after the bearings were measured.The result is that after step k + 1, only Z k+1 , f z i k +1 ; z j k +1 g remain in the measurement bu er for the next step.

Updating the relative map
At time k, the algorithm checks the bu er to nd all pairs of consecutive bearing measurements of the same landmark fz i k,1 ; z j k g .These pairs create a list of observable invariant features which are checked against the invariant features already in the relative map.This check is necessary because not every landmark pair distance or landmark triplet angle appear in the relative map.Once a list of candidate features is found, a decision must be made as to which feature should be updated.We h a v e adopted a greedy strategy where the observable relative feature dij k with the highest covariance ij k is updated.The selected feature is updated using the familiar equations where dij k and ij k are the estimate and variance at time k and D ^i k+1 ; ^j k+1 is the new measurement, with variance D .

Updating the absolute map
The update of the absolute map is independent of the update of the relative map and can be done asynchronously.The upate step can be invoked automatically whenever the relative map is updated or can be delayed until a localization is required, a relative feature estimate changes dramatically, or some other criterion.
The update is done by optimizing the relationships between landmarks in the absolute map with respect to the relationships estimated in the relative map.This is done by applying Levenberg-Marquardt LM optimization to the cost function where F k is the number of features in the relative map at time k and ip and jp are the indeces for the landmarks which are the control points for the p th feature.
The LM algorithm has an advantage over the EKF or IEKF i.e.Newton-Raphson update in cases where inverse Hessian methods cause iterations to diverge away from the solution.The LM algorithm checks the solution at each step by e v aluating 12 and if the solution is worse, LM smoothly transitions to gradient descent behavior by forcing the Hessian to be diagonally dominant 7 .One step of LM has the same complexity as one step of the EKF or IEKF.

Memory and computational requirements
At time k, let L k be the number of landmarks in the absolute map, F k be the number of features in the relative map and V k be the number of landmarks that are visible.In the worst case, i.e. for small areas or long range sensors where every landmark is visible at each step, V k could be L k , but generally V k L k .With a reasonable triangulation, F k is OL k .
Checking for new features to add is fast.A list is maintained containing all of the landmarks which require another invariant feature in order to be properly constrained.This list is usually much smaller than L k .Determining whether a new landmark is to be added is OV k , since all visible landmarks must be checked to see if any are new.Determining which i n v ariant feature to update is OF k in the worst case.Once the feature to be added or updated is selected, the update is O1.OF k memory is required to store the relative map since a full covariance matrix is not needed.
The update of the absolute map given the features in the relative map is done using Levenberg-Marquardt optimization.One iteration is OL 3 k in general, but may be faster if sparse matrix techniques are used to speed up the inversion of the Hessian.The algorithm has quadratic convergence near the optimum, so although we require an unknown number of iterations there should not be many if the solution is near the optimum when iteration begins.We also do not need to update the absolute map every cycle.OL memory is required to store the absolute map.By comparison, Kalman ltering requires an OL 3 k update each time new observations become available, and OL 2 k memory to maintain the covariance matrix.

Experimental Results
The above algorithm has been implemented and tested in simulation and on real data.In the interest of time we will only present results with real data here.The robot is an RWI ATRV with an omnidirectional camera onboard.The robot logs images from the camera at about 1Hz, and odometry at about 10 Hz.The landmarks" are large textured foam blocks.It is not our intent at this time to solve the tracking or landmark recognition problem, so the landmark selection in the omnidirectional images was done by hand, and the visual tracking requires a user to identify targets.During these tests, the ATRV had a broken wheel.The result was a signi cant bias in dead reckoning which w as partially calibrated out.The error model was also modi ed to increase the vehicle motion uncertainty t o a m uch higher degree when going through large turns.The invariant ltering algorithm successfully recovers the relationships between the landmarks even with this bias.Figure 5 and 6 show the time evolution of invariant features d 01 and d 23 , the rst and fourth features to be added to the relative map.The estimated mean is Ground truth for the map is not available.Instead, we implemented a full minimum mean square error MMSE batch estimation and used all available data to optimize the estimate of map and robot trajectory.Since the map computed by the invariant lter is not computed with an absolute position reference, there is an unknown planar transformation between the map from the invariant lter and the map from the MMSE estimate.This is accounted for by nding the transformation which most closely aligns the two maps.The map recovered by the invariant lter is shown in Figure 7 along with the MMSE map.The landmark positions agree to within about a half meter after registration.

Summary and Conclusions
In this paper we i n troduce the invariant lter, a solution to the SLAM problem which decouples the robot position and orientation from the mapping phase in order to simplify computation.The strength of the invariant ltering approach to SLAM lies in the decoupling of the estimates of each e n vironment feature so that the map states are completely uncorrelated with each other and the robot state.This allows much more scalable approach since the computational and memory requirements scale linearly with the number of features.Our implementation has been shown to work onboard a mobile robot in an environment with a modest number of features.
Our current implementation requires some investigation into more reasonable triangulation schemes in order to achieve good maps.The current greedy triangulation does not take geometry into account, and can add relationships which do not adequately constrain the positions of some landmarks.

Figure 1 :
Figure 1: Errors in estimates of rover pose and landmark position, in the global frame, are correlated

Figure 2 :
Figure 2: Environment features invariant t o r o v er pose are impervious to errors in pose estimate

Figure 3 :
Figure 3: Angles between two landmarks subtended at a third are invariant to robot pose 4.1 Data structure

Figure 4 :
Figure 4: Relationship between absolute map and relative map data structures

Figure 7 :
Figure 7: Final estimated map x's with MMSE estimate o's