Prueba discriminatoria para las características evaluadas

The navigation systems discussed above all use the a hidden Markov model— predominantly the Kalman filter—to combine inertial prediction steps with state measurement updates for aiding information sources. We will now look at meth- ods developed by the robotics community to navigate robots in GPS-denied envi- ronments.

2.5.1

Miniaturizing Digital Computers (Least Squares)

Over many centuries, cartographers had developed many localization and map- ping techniques but the advent of miniaturized computers had started a new au- tomated direction in navigation. During the 1980’s, Smith, Cheeseman [210] and

others help start the discussion on automating a localization and mapping frame- work. Their starting point — without human involvement — was that a robot would be switched on and have to navigate and map the world from nothing but the available (onboard) sensors, software, and computational resources. Their work [211, 212] was able to identify that either the mapping or localization could be done with some stochastic representation.

Leonard et al. [135] and others — in early the 1990’s — transitioned towards si- multaneous localization and mapping (SLAM), which could be achieved by writ- ing all sensor measurements in a relative reference frame. For example, instead of writing the navigation solution as the result of absolute measurements from rang- ing or bearing measurements to know landmarks, all position variables are kept fluid and measurements are only defined between them. Lu and Milios [141], in 1997, introduced a common notation based on Lie Groups and Algebra for a two dimensional simultaneous localization and mapping framework.

The ability to perform both localization and mapping, by analogy, allows the navigation system ”to pick itself up by its shoestrings”. Similarly in the early 1800’s, Gauss had formulated a system of equations in a relative coordinate frame to estimate the positions and distances between objects from bearing only geo- graphic surveys, and is embodied on the ten Deutsche Mark banknote shown in Fig. 2-3. Gauss developed a least squares solution, assuming normally dis- tributed measurement measurement errors, thereby estimating an multivariate normal posterior distribution to represent the uncertainty in position estimates.

Figure 2-3: Deutsche Ten Mark showing a relative coordinate frame optimization problem, bottom right, in honor of Gauss for mapping and localizing positions from bearing only measurements.

During the same period, starting with the Apollo lunar missions, techniques such as bundle adjustment (BA) and structure from motion (SfM) had been de- veloped to build three dimensional reconstructions of from photographic data, as shown by the work of Brown in the mid 1970’s [26]. A modern look at Bun- dle Adjustment and Structure from Motion techniques was published by Triggs et al. [231] in 2000, and heavily focused on parametric optimization techniques to perform inference on large scale photometric data. These techniques use Gaussian parametric representations with a Quasi-Newton iterative optimization process to infer maximum likelihood estimates (MLE) for variable assignments. Section 2.6 discusses parametric optimization in the context of factor graphs in more detail.

2.5.2

EKF-SLAM

The discussion above shows how our understanding develop from using abso- lution navigation measurements, from a GPS for example, had to evolve into a relative frame representation. With a relative frame representation, we can write down how wheel odometry develops a rigid transform between two pose loca- tions as a robot is traveling through the world. The next question was how to use relative frame mechanizations to develop an online state estimate in the location of the robot, with the additional by product of a local map of the surrounding environment.

Following success from INS/GPS or INS/DVL Kalman filtering solutions for multi-sensor fusion processes, a natural step was to use an extended Kalman filter to infer state estimates for simultaneous localization and mapping, [212]. Many authors, including Leonard et al. [135] , Bailey [10] and Durrant-Whyte [11, 48], resorted to an augmented state vector EKF approach. Previous pose positions and landmark locations in some world reference frame would be stacked into a large augmented state vector Θ =X1 X2, ..., L1, ....

A trivial EKF transition model is used as all variables are assumed static in the world frame, but a large measurement function would be used to produce resid- uals z(k) between predicted ˆy(k) = h Θ(k)
and actually observed measurements

˜ y(k)

δ(k)= z(k)= ˜y(k)− h Θ(k)
. (2.8)

Note, the problem time step counter ·(k). With measurement residuals in hand, a

filter update equation:

Θ(k)+ ← Θ(k)−+ K z(k) (2.9)

The major cost of the EKF-SLAM algorithm is in inverting the high dimension state covariance matrix P for each Kalman measurement update step. Similar to the MSC-KF, the computational cost could be reduced by Rao-Blackwellized land- mark states – that is removing landmark variables from the Kalman filter state, and iteratively optimizing landmarks relative to current pose states. Pose states are then resolved, through Kalman filter update models, according to the latest landmark state estimates. Double counting of information can be avoided with a null-projection, as done by Mourikis et al. [158].

2.5.3

Sparse Information Filters

The next major development in SLAM came in the early 2000’s with the realization that inverting the state covariance matrix P would result in a sparse information matrix, P−1. Methods, such as Thrun et al. [225] or Eustice et al. [52], developed SLAM based algorithms directly in the inverted information space, greatly reduc- ing the cubic computational cost of inverting the covariance matrix at each step.

The problem with information state filters is difficulty in tracking which terms of the information matrix should be kept, and which should be discarded since the inherent structure of the matrix was not yet obvious. New developments would soon show that the hidden Markov model assumption was an over simplification, and that far better computational performance could be achieved while still com- puting the exact solution for a Gaussian distributed system.

2.5.4

Monte Carlo Localization

Another facet to HMM-type localization and mapping approaches was inspired by particle filtering methods. Monte Carlo localization, by Dellaert et al. [43], es- timates the posterior belief of the latest pose and landmark states using a particle filter approach. The approach allows non-Gaussian beliefs to be used for measure- ment and prediction update cycles to the state vector. These approaches suffered from particle depletion in high dimensions, and therefore maintained relatively small augmented state vectors.

To improve on depletion, the FastSLAM [226] approach is conceptually sim- ilar by emphasizing a subset of Gaussian parametric solutions. FastSLAM is a

midway between belief methods and multi-hypothesis parametric solutions. In FastSLAM, a set of particles are used to approximate the belief of all possible tra- jectories, where each particle represents an entire trajectory.

All trajectory poses and landmark positions are stacked in one vector, and each vector represents one particle. Each vector is then augmented with new pose infor- mation, using the odometry model, to update the entire system. Multi-hypothesis approaches therefore would require exponentially many particles to explore the entire space of possible solutions. FastSLAM offloads the problem of hypothe- sis selection to the user, and internally loses hypotheses at the resampling step. Lost modes cannot be recovered, and represents a fundamental difference in the method for multi-modal inference.

Furthermore, the augmented state variables can be Rao-Blackwellized, such that landmarks are separated out and estimated in an iterative fashion alongside the pose state variables. For example, under correct data association, a single parti- cle can be used to recover the full SLAM solution. However, FastSLAM does not ex- ploit structure within the joint probability distribution, unlike the Bayes tree [115]. Critically, the Bayes tree precisely encodes the type of structure needed for multi- hypothesis tracking, but this has not previously been studied in detail.

While the FastSLAM approach is able to track non-Gaussian belief, each vari- ation across the entire trajectory and landmark positions requires a different par- ticle. In turn, exponentially many particles are needed to encompass all the vari- able dimensions for an accurate posterior estimate – this is prohibitively ineffi- cient. Since belief complexity scales exponentially in the dimension of a particle, one would rather use more particles who each represent a much smaller state vec- tor. Our approach will instead work on a particles per marginal belief basis, where marginal belief dimension is kept small.