Sensor Fusion in Certainty Grids
      for Mobile Robots

 Hans P. Moravec
 Robotics Institute
 Carnegie-Mellon University

 Pittsburgh, PA  15213  USA

 March 1988


\section{Abstract}

	A numerical representation of uncertain and incomplete sensor
knowledge we call Certainty Grids has been used successfully in
several of our past mobile robot control programs, and has proven
itself to be a powerful and efficient unifying solution for sensor
fusion, motion planning, landmark identification, and many other
central problems.  We had good early success with {\it ad-hoc}
formulas for updating grid cells with new information.  A new Bayesian
statistical foundation for the operations promises further
improvement. We propose to build a software framework running on
processors onboard our new Uranus mobile robot that will maintain a
probabilistic, geometric map of the robot's surroundings as it
moves. The "certainty grid" representation will allow this map to be
incrementally updated in a uniform way from various sources including
sonar, stereo vision, proximity and contact sensors.  The approach can
correctly model the fuzziness of each reading, while at the same time
combining multiple measurements to produce sharper map features, and
it can deal correctly with uncertainties in the robot's motion.  The
map will be used by planning programs to choose clear paths, identify
locations (by correlating maps), identify well known and
insufficiently sensed terrain, and perhaps identify objects by
shape. The certainty grid representation can be extended in the time
dimension and used to detect and track moving objects. Even the
simplest versions of the idea allow us fairly straightforwardly to
program the robot for tasks that have hitherto been out of reach. We
look forward to a program that can explore a region and return to its
starting place, using map "snapshots" from its outbound journey to
find its way back, even in the presence of disturbances of its motion
and occasional changes in the terrain.

\section{Introduction}

	Robot motion planning systems have used many space and object
representations.  Objects have been modeled by polygons and polyhedra,
or bounded by curved surfaces. Free space has been partitioned into
Vornoi regions or, more heuristically, free corridors.  Traditionally
the models have been hard edged - positional uncertainty, if
considered at all, was used in just a few special places in the
algorithms, expressed as a gaussian spread.  Partly this is the result
of analytical difficulty in manipulating interacting uncertainties,
especially if the distributions are not Gaussian.  Incomplete error
modeling reduces positional accuracy.  More seriously, it can produce
entirely faulty conclusions: a false determination of an edge in a
certain location, for instance, may derail an entire train of
inference about the location or existence of an object. Because they
neglect uncertainties and alternative interpretations, such programs
are brittle.  When they jump to the right conclusions, they do well,
but a small error early in the algorithm can be amplified to produce a
ridiculous action. Most artificial intelligence based robot
controllers have suffered from this weakness.

	We've built our share of brittle controllers. Occasionally,
however, we've stumbled across numerical (as opposed to analytic)
representations that seem to escape this fate. One is deep inside the
program that drove the Stanford Cart in 1979 [\cite{Moravec80}].  Each
of 36 pairings of nine images from a sliding camera produced a stereo
depth measurement of a given feature, identified by a correlator, in
the nine images.  Some pairings were from short baselines, and had
large distance uncertainty, others were from widely separated
viewpoints, with small spread. The probability distributions from the
36 readings were combined numerically in a 1000 cell array, each cell
representing a small range interval (Figure \ref{Nine}).  Correlator
matching errors often produced a multi-peaked resultant distribution,
but the largest peak almost always gave the correct range.  The
procedure was the most error tolerant step in the Cart navigator, but
it alone did not protect the whole program from brittleness.

\begin{figure}
\vspace{4in}
\caption[Nine Eyed Stereo]{\label{Nine}{\bf Nine Eyed Stereo - }
Identifications of a point on an object seen in nine different images
taken as a camera traversed a track at right angles to its direction
of view. Each pairing of images gives a stereo baseline, some short,
some long. Long baselines have less uncertainty in the calculated
distance.  The distributions for all 36 possible pairings are added in
a one dimensional ``certainty grid'', and the peak of the resultant
sum is taken as the actual distance to the object.  The top graph is
for a case where all nine identifications of the point in the images
are correct.  The bottom is a case where one image is in error.  The
error produces eight small peaks at incorrect locations, but these are
no match for the accumulation of correct values.}
\end{figure}

	A descendant of the Cart program by Thorpe and Matthies
contained a path planner [\cite{Thorpe84}] that modeled floor space as
a grid of cells containing numbers representing the suitability of
each region to be on a path.  Regions near obstacles had low
suitability while empty space was high.  A relaxation algorithm found
locally optimum paths (Figure \ref{Path}).  The program represented
uncertainty in the location, or even existence, of obstacles by having
the suitability numbers for them vary according to extended,
overlapping, probability distributions.  The method dealt very
reliably and completely with uncertainty, but also suffered from being
embedded in an otherwise brittle program.

\begin{figure}
\vspace{7in}
\caption[Relaxation Path Planner]{\label{Path}{\bf Relaxation Path
Planner - } A path is chosen that minimizes a given cost function in a
Certainty Grid.  Small perturbations are made in the vertices of the
path in directions that reduce the cost.}
\end{figure}

	Our earliest thorough use of a numerical model of position
uncertainty was in a sonar mapper, map matcher and path planner
developed initially for navigating the Denning Sentry
[\cite{Moravec85}, \cite{Elfes86}, \cite{Kadonoff86}]. Space was
represented as a grid of cells, each mapping an area 30 (in some
versions 15) centimeters on a side and containing two numbers, one the
estimated probability that the area was empty, the other that it was
occupied. Cells whose state of occupancy was completely unknown had
both probabilities zero, and inconsistent data was indicated if both
numbers were high.  Many of the algorithms worked with the differences
of the numbers.  Each wide angle sonar reading added a thirty degree
swath of emptiness, and a thirty degree arc of occupancy, by itself a
very fuzzy image of the world.  Several hundred readings together
produced an image with a resolution often better than 15 centimeters,
despite many aberrations in individual readings (Figure \ref{Sonar}).
The resiliency of the method was been demonstrated in successful
multi-hour long runs of Denning robots around and around long
trajectories, using three second map building and three second map
matching pauses at key intersections to repeatedly correct their
position.  These runs worked well in clutter, and survived
disturbances such as people milling around the running robot.

\begin{figure}
\vspace{7in}
\caption[Sonar Mapping and Navigation]{\label{Sonar}
{\bf Sonar Mapping and Navigation - }
Plan view of the certainty grid built by a sonar guided robot
traversing our laboratory.  The scale marks are in feet. Each point on
the dark trajectory is a stop that allowed the onboard sonar ring to
collect twenty four new readings. The grid cells are white if the
occupancy probability is low, dots if unknown, and $\times$ if high.
The forward paths were planned by an A* algorithm working in the grid
as it was incrementally generated.}
\end{figure}

	Ken Stewart of MIT and Woods Hole has implemented a three
dimensional version of the sonar mapper for use with small submersible
craft.  Initially tested in simulation in the presence of large
simulated errors, Stewart's program provided extremely good
reconstructions, in a $128 \times 128 \times 64$ array, of large scale
terrain, working with about $60,000$ readings from a sonar transducer
with a seven degree beam.  Running on a Sun computer, his program can
processed sonar data fast enough to keep up with the approximately one
second pulse rate of the transducers on the two candidate submersibles
at Woods Hole.  The program was recently tested on real sonar data
from a scanning transducer on a underwater robot that swam over the
remains of the civil war battleship USS Monitor as part of a NOAA/Navy
survey [\cite{Stewart88}, \cite{Stewart87}]. The impressive results
are shown in Figure \ref{Monitor}.

	Recently Serey and Matthies demonstrated the utility of the
grid representation in a stereo vision based navigator running on our
``Neptune'' (Figure \ref{Neptune}) mobile robot [\cite{Serey86}].
Edges crossing a particular scanline in the two stereo images are
matched by a dynamic programming method, to produce a range profile.
The wedge shaped space from the camera to the range profile is marked
empty, cells along the profile itself are marked occupied.  The
resulting map is then used to plan obstacle avoiding paths as with the
stereo and sonar programs mentioned above (Figure \ref{Stereo}).

\begin{figure}
\vspace{7.25in}
\caption[Stereo Mapping and Navigation]{\label{Stereo}
{\bf Stereo Mapping and Navigation - }
Plan view of the certainty grid built by a stereo guided robot
traversing our laboratory.  The situation is analogous to the sonar
case of Figure \ref{Sonar}, but the range profiles were gathered from
a scanline stereo method using two TV cameras rather than a sonar
ring.}
\end{figure}

	Matthies and Elfes combined improved versions of the sonar and
stereo programs into a single one that builds maps integrating data
from both sensors [\cite{ElfMat87}].  Their first results, also from a
run of Neptune, are shown in figure \ref{Steson}.

\begin{figure}
\vspace{7.25in}
\caption[Stereo and Sonar Sensor Integration]{\label{Steson}
{\bf Stereo and Sonar Sensor Integration - }
Plan view of the certainty grids built on the first and tenth steps of
a dual sensor run. The leftmost grids contain sonar data only, the
center grid has stereo vision only, the rightmost is the combination
of the two. Occupied regions are marked by shaded squares, empty areas
by dots fading to white, and unknown territory by $+$ signs.}
\end{figure}

	In work in progress, In So Kweon of Carnegie-Mellon University
has successfully demonstrated mapping of data from an ERIM scanning
laser rangefinder into a three dimensional grid [\cite{Kweon87}].

	Despite its effectiveness, in each instance we adopted the
grid representation of space reluctantly.  This may reflect habits
from a recent time when analytic approaches were more feasible and
seemed more elegant because computer memories were too small to easily
handle numerical arrays of a few thousand to a million cells.  I think
the reluctance is no longer appropriate.  The straightforwardness,
generality and uniformity of the grid representation has proven itself
in finite element approaches to problems in physics, in raster based
approaches to computer graphics, and has the same promise in robotic
spatial representations.  At first glance a grid's finite resolution
seems inherently to limit positioning accuracy.  This impression is
false.  Cameras, sonar transducers, laser scanners and other long
range sensors have intrinsic uncertainties and resolution limits that
can be matched by grids no larger than a few hundred cells on a side,
giving a few thousand cells in two dimensions, or a few million in
three dimensions.  Since the accuracy of most transducers drops with
range, even greater economy is possible by using a hierarchy of
scales, covering the near field at high resolution, and successively
larger ranges with increasingly coarser grids. Besides this, the
implicit accuracy of a certainty grid can be better than the size of
its cell.  The grid can be thought of as a discrete sampling of a
continuous function.  Extended features such as lines (perhaps
representing walls) may be located to high precision by examining the
parameters of surfaces of best fit.  The Denning robot navigator
mentioned above convolves two maps to find the displacement and
rotation between them.  In the final stages of the matching
correlation values are obtained for a number of positions and angles
in the vicinity of the best match.  A quadratic least squares
polynomial is fitted to the correlation values, and its peak is
located analytically.  Controlled tests of the procedure usually give
positions accurate to better than one quarter of a cell width.

	Our results to date suggest that many mobile robot tasks can
be solved with this unified, sensor independent, approach to space
modeling.  The key ingredients are a robot centered, multi resolution,
map of the robot's surroundings, procedures for efficiently inserting
data from sonar, stereo vision, proximity and other sensors into the
map, other procedures for updating the map to reflect the
uncertainties introduced by imprecise robot motion, and yet others to
extract conclusions from the maps.  We've already demonstrated
procedures that produce local and global navigational fixes and
obstacle avoiding paths from such maps.  Other tasks, such as tracking
corridors, finding vantage points with good views of unseen regions,
and identification of larger features such as doors and desks by
general shape seem within reach.

\begin{figure}
\vspace{4in}
\caption[3D Underwater Sonar Image of USS Monitor]{\label{Monitor}
{\bf Sonar Image of USS Monitor - }
A surface has been extracted from a three dimensional certainty grid
$128$ cells on a side. The grid was built from approximately $100,000$
readings from a $1.5\deg$ scanning sonar on a free swimming robot. The
ship is lying upside down, with many parts of the hull collapsed.}
\end{figure}

\section{The Representation}

	The sonar mappers mentioned above are our most thorough use to
date of the certainty grid idea.  Although our original
implementations used two grids to represent occupancy knowledge
(labeled $P_{occupied}$ and $P_{empty}$), Stewart's 3D system used
only one.  An analysis of the steps in our code revealed that one grid
did indeed suffice, and this simplification made clear several
puzzling issues in the original formulation. Here I will present a
sample of the methods we have used in most of our work so far.  A new,
Bayesian, approach follows after.

	Before any measurements were made, the grid was initialized to
a background occupancy certainty value, $Cb$. This number represented
the average occupancy certainty we expected in a mature map, and
encoded a (very) little bit of a-priori information we had about the
world.  In our lab a good $Cb$ seemed to be about the number of cells
in the perimeter of the grid divided by the total cells ($4 \times 32
/ (32 \times 32) = 1/8$) in the case of the Denning code. If the space
was very cluttered, $Cb$ should be larger.  As the map was used,
values near $Cb$ would indicate regions whose occupancy state was
essentially unknown, while those much nearer zero would represent
empty places, and those much nearer unity were likely to be occupied.
Most of the planning algorithms that used the grid are better off if
they do not make sharp distinctions, but instead numerically combine
the certainty values from various cells to produce ``goodness of fit''
numbers for their various hypotheses.  In this way the essential
uncertainties in the measurements are not masked, and the algorithms
do not jump to unnecessary, possibly false, conclusions.

\section{Inserting Measurements}

	The readings of almost any kind of sensor can be incorporated
into a certainty grid, if they can be expressed in geometric terms.
The information from a reading can be as minimal as a proximity
detector's report that there is probably something in a certain region
of space, or as detailed as a stereo depth profiler's precise numbers
on the contours of a surface.

	The first step, in general, is to express the sensor's
measurement as a numerical spatial certainty distribution commensurate
with the grid's geometry.  For an infrared proximity detector this may
take the form of set of numbers $P_x$ in an elliptical envelope with
high certainty values in a central axis (meaning detection is likely
there) tapering to zero at the edges of the illumination envelope.
Let's suppose the sensor returns a binary indication that there is or
is not something in its field of view.  If the sensor reports a hit,
cells in the certainty grid $C_x$ falling under the sensor's envelope
can be updated with the formula
$$ C_x := C_x + P_x - C_x \times P_x $$
which will increase the $C$ values.  In this case the $P$ values
should be scaled so their sum is one, since the measurement describes
a situation where there is something somewhere in the field of view,
probably not everywhere.  If the reliability of the sensor is less
than perfect, the normalization may be to a sum less than unity.  If,
on the other hand, the detector registers no hit, the formula might be
$$ C_x := C_x \times ( 1 - P_x ) $$
and the $C$s will be reduced. In this case the measurement states that
there is nothing anywhere in the field of view, and the $P$ values
should reflect only the chance that an object has been overlooked at
each particular position; i.e. they should not be normalized.  If the
sensor returns a continuous value rather than a binary one, perhaps
expressing some kind of rough range estimate, a mixed strategy similar
to the one described below for sonar is called for.

	A Polaroid sonar measurement is a number giving the range of
the nearest object within an approximately thirty degree cone in front
of the sonar transducer.  Because of the wide angle, the object
position is known only to be somewhere on a certain surface.  This
range surface can be handled in the same manner as the sensitivity
distribution of a proximity detector ``hit'' above. The sonar
measurement has something else to say, however.  The volume of the
cone up to the range reading is probably empty, else a smaller range
would have been returned.  The empty volume is like the ``no hit''
proximity detector case, and can be handled in the same fashion.  So a
sonar reading is like a proximity detector hit at some locations, and
increases the occupancy probability there, and like a miss at others,
where it decreases the probability.  If we have a large number of
sonar readings taken from different vantage points (say as the robot
moves), the gradual accumulation of such certainty numbers will build
a respectable map. We can, in fact, do a little better than that.
Imagine two sonar readings whose volumes intersect.  And suppose the
``empty'' region of the second overlaps part of the range surface of
the first.  Now the range surface says ``somewhere along here there is
an object'', while the empty volume says ``there is no object here''.
The second reading can be used to reduce the uncertainty in the
position of the object located by the first reading by decreasing the
probability in the area of the overlap, and correspondingly increasing
it in the rest of the range surface.  This can be accomplished by
reducing the range surface certainties $R_x$ with the formula $R_x :=
R_x \times (1 - E_x)$ where $E_x$ is the ``empty'' certainty at each
point from the second reading, then normalizing the $R$s. This method
is used to good effect in the existing sonar navigation programs, with
the elaboration that the $E$s of many readings are first accumulated,
and then used to condense the $R$s of the same readings. (It is this
two stage process that led us to use two grids in our original
programs. In fact, the grid in which the $E$s are accumulated need
merely be temporary working space.)

	The stereo method of Serey and Matthies provides a depth
profile of visible surfaces.  Although, like a sonar reading, it
describes a volume of emptiness bounded by a surface whose distance
has been measured, it differs by providing a high certainty that there
is matter at each point along the range surface.  The processing of
the ``empty'' volume is the same, but the certainty reduction and
normalization steps we apply to sonar range surfaces are thus not
appropriate.  The grid cells along a very tight distribution around
the range surface should simply be increased in value according to the
``hit'' formula.  The magnitude and spread of the distribution should
vary according to the confidence of the stereo match at each
point. The method used by Serey and Matthies matches edge crossing
along corresponding scanlines of two images, and is likely to be
accurate at those points.  Elsewhere it interpolates, and the expected
accuracy declines.

	If the robot has proximity or contact sensors, its own motion
can contribute to a certainty grid.  Areas traversed by the robot are
almost certainly empty, and their cells can be reduced by the ``no
hit'' formula, applied over a confident sharp edged distribution in
the shape of the robot.  This approach becomes more interesting if the
robot's motion has inherent uncertainties and inaccuracies.  If the
certainty grid is maintained so it is accurate with respect to the
robot's present position (so called robot co-ordinates), then the past
positions of the robot will be uncertain in this co-ordinate
system. This can be expressed by {\it blurring} the certainty grid
accumulated from previous readings in a certain way after each move,
to reflect the uncertainty in that move. New readings are inserted
without blur (essentially the robot is saying ``I know {\it exactly}
where I am now; I'm just not sure where I was before).  The track in
the certainty grid of a moving robot's path in this system will
resemble the vapor trail of a high flying jet - tight and dense in the
vicinity of the robot, diffusing eventually to nothing with time and
distance.

\begin{figure}
\vspace{5in}
\caption[The Neptune Mobile Robot]{\label{Neptune}
{\bf The Neptune Mobile Robot - }
Host for many early Certainty Grid experiments.}
\end{figure}

\section{Extracting Deductions}

	The purpose of maintaining a certainty grid in the robot is to
plan and monitor actions.  Thorpe and Elfes showed one way to plan
obstacle avoiding paths.  Conceptually the grid can be considered an
array of topographic values - high occupancy certainties are hills
while low certainties are valleys. A safe path follows valleys, like
running water.  A relaxation algorithm can perturb portions of a trial
path to bring each part to a local minimum.  In principle a decision
need never be made as to which locations are actually empty and which
are occupied, though perhaps the program should stop if the best path
climbs beyond some threshold ``altitude''.  If the robot's sensors
continue to operate and update the grid as the path is executed,
impasses will become obvious as proximity and contact sensors raise
the occupancy certainty of locations where they make contact with
solid matter.

	As indicated in the introduction, we have already demonstrated
effective navigation by convolving certainty grids of given locations
built at different times, allowing the robot to determine its location
with respect to previously constructed maps. This technique can be
extended to subparts of maps, and may be suitable for recognizing
particular landmarks and objects.  For instance, we are presently
developing a wall tracker that fits a least squares line to points
that are weighted by the product of the occupancy certainty value and
a gaussian of the distance of the grid points from an a-priori guess
of the wall location.  The parameters of the least squares line are
the found wall location, and serve, after being transformed for robot
motion, as the initial guess for the next iteration of the process.

	For tasks that would benefit from an opportunistic exploration
of unknown terrain, the certainty grid can be examined to find
interesting places to go next.  Unknown regions are those whose
certainty values are near the background certainty $Cb$.  By applying
an operator that computes a function such as $$\sum (C_x - Cb)^2$$
over a weighted window of suitable size, a program can find regions
whose contents are relatively unknown, and head for them.  Other
operators similar in spirit can measure other properties of the space
and the robot's state of knowledge about it.  Hard edged
characterizations of the stuff in the space can be left to the last
possible moment by this approach, or avoided altogether.

\section{A Plan: Awareness for a Robot}

	Uranus is the CMU Mobile Robot Lab's latest and best robot and
the third and last one we intend to construct for the foreseeable
future.  About 60 cm square, with an omnidirectional drive system
intended primarily for indoor work, Uranus carries two racks wired for
the industry standard VME computer bus, and can be upgraded with off
the shelf processors, memory and input output boards.  In the last few
years the speed and memory available on single boards has begun to
match that available in our mainframe computers. This removes the main
arguments for operating the machine primarily by remote control.  With
most computing done on board by dedicated processors, enabling very
high bandwidth and reliable connection of processors to sensors and
effectors, real time control is much easier.  Also favoring this
change in approach is a realization by us, growing from our experience
with robot control programs from the very complex to the relatively
simple, that the most complicated programs are probably not the most
effective way to learn about programming robots. Very complex programs
are slow, limiting the number of experiments possible in any given
time, and they involve too many simultaneous variables, whose effects
can be hard to separate.  A manageable intermediate complexity seems
likely to get us to our long term goals fastest.  The most exciting
element in our current plans is a realization that certainty grids are
a powerful and efficient unifying solution for sensor fusion, motion
planning, landmark identification, and many other central problems.

\begin{figure}
\vspace{4in}
\caption[The Uranus Mobile Robot]{\label{Uranus}
{\bf The Uranus Mobile Robot - }
Great Expectations.}
\end{figure}

	As the core of the robot and the research we will prepare a
kind of operating system based on the "certainty grid" idea.  Software
running continuously on processors onboard Uranus will maintain a
probabilistic, geometric map of the robot's surroundings as it moves.
The certainty grid representation will allow this map to be
incrementally updated in a uniform way from various sources including
sonar, stereo vision, proximity and contact sensors.  The approach can
correctly model the fuzziness of each reading, while at the same time
combining multiple measurements to produce sharper map features, and
it can deal correctly with uncertainties in the robot's motion.  The
map will be used by planning programs to choose clear paths, identify
locations (by correlating maps), identify well known and
insufficiently sensed terrain, and perhaps identify objects by shape.
To obtain both adequate resolution of nearby areas and sufficient
coverage for longer range planning, without excessive cost, a
hierarchy of maps will be kept, the smallest covering a 2 meter area
at 6.25 cm resolution, the largest 16 meters at 50 cm resolution
(Figure \ref{Maps}).  This map will be ``scrolled'' to keep the robot
centered as it moves, but rotations of the robot will be handled by
changing elements of a matrix the represents the robot's orientation
in the grid.  The map forms a kind of consciousness of the world
surrounding the robot - reasoning about the world would actually be
done by computations in the map.  It might be interesting to take one
more step in the hierarchy, to a one meter grid that simply covers the
robot's own extent. It would be natural to keep this final grid
oriented with respect to robot chassis itself, rather than
approximately to the compass as with the other grids.  This change of
co-ordinate system would provide a natural distinction between
``world'' awareness and ``body'' or ``self'' awareness.  Such encoding
of a sense of self might even be useful if the robot were covered with
many sensors, or perhaps were equipped with manipulators. We have no
immediate plans in that direction, and so will pass by this
interesting idea for now.

\begin{figure}
\vspace{4in}
\caption[Map Hierarchy]{\label{Maps}
{\bf Map Resolution Hierarchy - }
Coarse maps for the big picture, fine ones for the fiddly details in
the immediate environment.  All the maps are scrolled to keep the
robot in the center cells.}
\end{figure}

	Our initial version will contain a pair of two dimensional
grid sets, one mapping the presence of objects at the robots operating
height of a few feet above ground level.  The other will map the less
complex idea of presence of passable floor at various locations.  The
object map will be updated from all sensors, the floor map primarily
from downward looking proximity detectors, though possibly also from
long range data from vision and sonar.  The robot will navigate by
dead reckoning, integrating the motion of its wheels. This method
accumulates error rapidly, and this uncertainty will be reflected in
the maps by a repeated blurring operation. Old readings, whose
location relative to the robot's present position and orientation are
known with decreasing precision, will have their effect gradually
diffused by this operation, until they eventually evaporate to the
background certainty value.

	It would be natural to extend the two-grid system to many
grids, each mapping a particular vertical slice, until we have a true
three dimensional grid. We will do this as our research results, and
processing power permit.  The availability of single board array
processors that can be installed on the robot would help this, as the
certainty grid operations are very amenable to vectorizing. The
certainty grid representation can also be extended in the time
dimension, with past certainty grids being saved at regular intervals,
like frames in a movie film, and registered to the robot's current
co-ordinates (and blurred for motion uncertainties).  Line operators
applied across the time dimension could detect and track moving
objects, and give the robot a sense of time as well as space.  This
has some very thrilling conceptual (and perceptual) consequences, but
we may not get to it for a while.

	Even the simplest versions of the idea will allows us fairly
straightforwardly to program the robot for tasks that have hitherto
been out of reach. We look forward to a program that can explore a
region and return to its starting place, using map "snapshots" from
its outbound journey to find its way back, even in the presence of
disturbances of its motion and occasional changes in the terrain.  By
funneling the sensor readings through a certainty grid, which collects
and preserves all the essential data, and indications of
uncertainties, and makes it available in a uniform way, we avoid the
problem we've had, that for each combination of sensor and task a
different program is required.  Now the task execution is decoupled
from the sensing, and thus becomes simpler.

\def\n{\overline}
\def\o{{\bf o}}
\def\p{{\bf p}}
\def\P{{\bf P}}

\section{Bayesian Reasoning}

	In most of our work to date, we have used {\it ad-hoc}
formulas to update the certainty grid estimates. Recently a less
arbitrary statistical approach derived from Bayes theorem
[\cite{Berger85}] has captured our attention.  Preliminary results
using this approach are at least as good as those from the old
formulas.  Many puzzling aspects of the old scheme have been clarified
in the process.

	Let $\p(A|B)$ represent our best estimate of the likelihood of
situation A if we have received information B. By definition 
\begin{equation}
\p(A|B) =
{{\p(A\land B)}\over{\p(B)}}
\end{equation}
 Plain $\p(A)$ represents our estimate of $A$ given no special
information.  The alternative to event $A$ will be referred to as
$\n{A}$ (not $A$). For any $B$
\begin{equation}
\p(A|B) + \p(\n{A}|B) = 1
\end{equation}

	A certainty grid is a regular finite element model of space.
Each cell of the grid contains a likelihood estimate (our
``certainty'') of a given property of the corresponding region of
space.  Primarily we are concerned with simple occupancy of the
region, represented by $\p(\o [x])$, the probability that region $x$
is occupied.  When a discussion contains only one particular $x$, we
will drop the subscript, and refer simply to $\p(\o)$.

	We will be considering data derived from wide angle sonar
range measurements.  A given measurement will be designated $M[i]$,
with $i$ being the sequential number of the reading.  The intersection
of a set of readings can be designated by a range in subscript, as in
\begin{equation}
		M[<n] = \bigcap_{i=1}^{i<n} M[i]
\end{equation}
 or by a list as in 
\begin{equation}
		M[i,j,l] = M[i]\land M[j]\land M[l]
\end{equation}
When only one reading $M[i]$ enters into a discussion, we will
abbreviate its name to simply $M$.  Each measurement has a value, a
sonar range $R(M)$.  The sonar sensor is quantized, and $R$ will be an
integer.

	$\P(\o)$ is the probability that any particular cell is
occupied, i.e. the average occupation density of our space.  Our
measurements don't give $\P(\o)$ directly, but it is approximately the
overall average of the $\p(\o)$'s of all the cells of a typical map of
the space.  By definition, $\P(\n{\o}) = \n{\P(\o)} = 1 - \P(\o)$.

\section{Fundamental Formulas}

	For two occupancy possibilities $\o$ and $\n{\o}$ of a cell, new
information $B$ and old information $A$, one form of Bayes theorem gives:
\begin{equation}
\p(\o|B\land A) = {{\p(B|\o\land A)\times{\p(\o|A)}}
 \over{\p(B|\o\land A)\times{\p(\o|A)}+
 \p(B|\n{\o}\land A)\times{\p(\n{\o}|A)}}}	\label{bay}
\end{equation}
\begin{equation}
\p(\n{\o}|B\land A) = 
 {{\p(B|\n{\o}\land A)\times{\p(\n{\o}|A)}}
 \over{\p(B|\o\land A)\times{\p(\o|A)}+
 \p(B|\n{\o}\land A)\times{\p(\n{\o}|A)}}}	\label{bayn}
\end{equation}
the ``odds'' formulation of this is compact and convenient for
computation, and will be important later:
\begin{equation}
{{\p(\o|B\land A)}\over{\p(\n{\o}|B\land A)}} =
 {{\p(B|\o\land A)}\over{\p(B|\n{\o}\land A)}}\times
 {{\p(\o|A)}\over{\p(\n{\o}|A)}}	\label{bayodds}
\end{equation}

	Formulas \ref{bay} and \ref{bayn} are somewhat complicated for
repeated use. Formula \ref{bayodds} is better, it is formulated in
terms of a product of odds.  When the odds ratio involves a
probability and its complement, odds and probabilities can be
interconverted by the relationship:

\begin{equation}
Odds(A) = {{\p(A)}\over{\p(\n{A})}}  =  {{\p(A)}\over{1 - \p(A)}}
\end{equation}
\begin{equation}
\p(A) = {{Odds(A)}\over{1 + Odds(A)}}
\end{equation}

	The $\p(B|\ldots)$ ratio in \ref{bayodds} is not of this
form. To compute its value, both numerator and denominator must be
evaluated separately. To make apparent this difference, the ratio
$\p(B|\o\land A)/\p(B|\n{\o}\land A)$ will be referred to as
$Odds(B||\o\land A)$.  Once the ratio is obtained, however, it can be
treated as any other odds number.

	If we deal exclusively with odds, all the combining operations
become multiplications.  Formula \ref{bayodds} is expressed:
\begin{equation}
	Odds(\o|A\land B) = {Odds(B||\o\land A)\times Odds(\o|A)}
\end{equation}

	An additional transformation streamlines the computation
more.  Let $L(A)$ represent the logarithm, to some suitable base,
of $Odds(A)$.  The formula then becomes a simple addition.
\begin{equation}
		L(\o|A\land B) = L(B||\o\land A) + L(\o|A)
\end{equation}

	The terms can be integers if the base of the logarithms is
chosen well.  Perfect certainty ($\p(\o) = 0$ and $\p(\o) = 1$) can no
longer be represented, but such values are probably a mistake in any
representation, since they are unalterable by any input.  Only God
should be absolutely certain of anything!

\section{Combining Formula}

	Bayes' theorem is a formula that combines independent sources
of information $A$ and $B$ into an estimate of a single quantity
$\p(\o|A\land B)$.  The new information $B$, occurs in terms of the
probability of $B$ in the situation that (in our case of interest) a
particular cell is or is not occupied, $\p(B|\o)$ and $\p(B|\n{\o})$.
This inversion is central to the usefulness of Bayes' theorem.  But
consider the problem of generating a map from information $A$ and $B$
when each has already been individually processed into a map,
i.e. find $\p(\o|A\land B)$ given $\p(\o|A)$ and $\p(\o|B)$.

	Bayes' formula (\ref{bayodds}) applied to information $B$ and
null $A$ (i.e. only global information from the $A$ side) makes the
relationship between $\p(\o|B)$ and $\p(B|\o)$ clear:

\begin{equation} \label{flip}					
{{\p(\o|B)}\over{\p(\n{\o}|B)}} = 
	{{\p(B|\o)}\over{\p(B|\n{\o})}}\times{{\P(\o)}\over{\P(\n{\o})}}
\end{equation}
thus
\begin{equation}
{{\p(B|\o)}\over{\p(B|\n{\o})}} = 
	{{\p(\o|B)}\over{\p(\n{\o}|B)}}\times
	{{\P(\n{\o})}\over{\P(\o)}}
\end{equation}

	It would be nice to substitute this ratio back into the
original version of formula (\ref{bayodds}) to produce a formula
giving $\p(\o|A\land B)$ in terms of $\p(\o|A)$ and $\p(\o|B)$, thus
allowing measurements $A$ and $B$ to be incorporated into maps
independently, and combined afterward.  This is not possible in
general because the two measurements may interact in some way - for
instance, either $A$ or $B$ alone may indicate a high probability of
$\o$, but taken together, they may confirm some other hypothesis, and
reduce the probability of $o$.  If, however, we make a strong
assumption of independence of B from A:
\begin{equation}
{{\p(B|\o\land A)}\over{\p(B|\n{\o}\land A)}} =
{{\p(B|\o)}\over{\p(B|\n{\o})}}		\label{independ}
\end{equation}
we can use equations (\ref{bayodds},\ref{flip},\ref{independ}) to
produce a {\it map combining formula}:
\begin{equation}
{{\p(\o|A\land B)}\over{\p(\n{\o}|A\land B)}} =
  {{\p(\o|B)}\over{\p(\n{\o}|B)}}\times
  {{\p(\o|A)}\over{\p(\n{\o}|A)}}\times
  {{\P(\n{\o})}\over{\P(\o)}}			\label{combine}
\end{equation}

	While the non-informative reading in equation \ref{bayodds},
that which leaves the original $\p(\o)$ estimate unchanged, is found
when $\p(B|\o)/\p(B|\n{\o}) = 1$, the non-informative case in equation
\ref{combine} happens when $\p(\o|B)/\p(\n{\o}|B) =
\P(\o)/\P(\n{\o})$, i.e.  when the cell density estimated from the
reading is the same as the average cell density of the whole map.

	Formula \ref{combine} is most important because it provides a
means for combining maps of the same area obtained by different means,
for instance by independent scans of different sensors.  It can also
be used to incorporate individual sensor readings as we build a map,
but is in general inferior to formula \ref{bayodds} for this purpose
because it precludes the use of any knowledge we may have of sensor
interactions.  In odds and log odds form it becomes:

\begin{equation}
Odds(\o|A\land B) = {{Odds(\o|A)\times Odds(\o|B)} \over {Odds(\o)}}
\end{equation}

\begin{equation}
	L(\o|A\land B) = L(\o|A) + L(\o|B) - L(\o)
\end{equation}

\section{Sonar Wedges}

	Simple reasoning from first principles can produce estimates
for $\p(M|\o)$ and $\p(M|\n{\o})$ for a sonar reading.  These values
can be used to update maps by direct application of the Bayes formula
\ref{bayodds}.  In a later section we will show how to systematically
incorporate in the possibility of errors in the readings.  For now
assume that the sensor always perfectly returns the range to the
nearest occupied cell within its angle of sensitivity. Define the
sonar regions according to the diagram:

\rule{0in}{3in}

	When incorporating a new reading $B$, the map built from prior
readings $A$ can be used to help provide an estimate for the
$\p(B|\o\land A)$ and $\p(B|\n{\o}\land A)$ of formula \ref{bayodds}.

	Let $\P(R)$ be the probability, prior to a the reading, that
the next sonar measurement will result in a given range $R$.  $\P(R)$
can be approximated by stepping through the possible ranges $R_1$,
$R_2$, \dots $R_n$, starting with the shortest, $R_1$, and multiplying
the occupancy probability $\p(\o|A)$ of each cell on the range surface
for that range by the probability that the sonar would detect an
occupied cell at that location.  The sum of these products at $R_1$,
call it $S(R_1)$, is $\P(R_1)$. Now $\P(R_2) =
S(R_2)\times(1-\P(R_1))$ since an echo from $R_2$ can happen only if
the sonar pulse has not already been intercepted at the shorter range
$R_1$. In general
\begin{equation} \label{profile}
	\P(R_i) = S(R_i) \times (1-{\sum_{j<i}\P(R_j)})
\end{equation}

	By definition $\sum\P(R)$ over all $R$ is unity.  We will
suggest later exactly how to compute $S$, and how the detection
probabilities required in the calculation above can be determined
empirically by collecting statistics from many maps of the correlation
of individual sensor readings with the composite maps they helped
create.

\subsection{External Region}

	Consider a cell in the external region.  $\p(M|\o)$ is our
estimate that a measurement range of $R(M)$, as opposed to some other
range, will occur if we happen to know only that the cell is occupied.
Since the cell is outside the sonar cone, its state of occupancy has
no effect on the reading, and we can refer only to the uniform range
distribution to conclude that

\begin{flushright}
[external region probabilities]
\end{flushright}

\begin{equation}
\p(M|\o) = \p(M|\n{\o}) = \P(R(M))
\end{equation}
and
\begin{equation} \label{equalprob}
{{\p(M|\o)}\over{\p(M|\n{\o})}} = 1
\end{equation}
inserting these into formula \ref{bayodds} gives
\begin{equation}\label{sonex}
{{\p(\o|M[1:i])}\over{\p(\o|M[1:i])}} = 
  {{\P(R(M))}\over{\P(R(M))}}\times
  {{\p(\o|M[<i])}\over{\p(\n{\o}|M[<i])}} =
{{\p(\o|M[<i])}\over{\p(\n{\o}|M[<i])}}
\end{equation}
so, the occupancy certainty is unchanged, as it should be, since the
sonar reading contains no information about the external cell.

\subsection{Range Surface}

	Now consider a cell on the range surface, and suppose this
surface covers n cells in all. By definition the range at this surface
is $R(M)$.

	If the cell is occupied, then a perfect sensor would detect
it, and so could not return a greater range than $R(M)$.  All ranges
beyond the occupied cell, thus, would be ``short circuited'' by the
cell.  It would, however, be possible for it to return shorter ranges,
if closer cells within the sonar beam angle happened to be occupied.
The probabilities of the shorter ranges should not be changed by the
presence of the farther occupied cell.  The probability of getting
just the range reading would be unity minus the a-priori probability
of getting a lesser reading, or equivalently, the a-priori probability
of getting a reading greater than or equal to $R(M)$.

\begin{flushright}
[range surface probabilities]
\end{flushright}

\begin{equation} \label{rangep}
\p(M|\o) = 1 - {\sum_{R<R(M)}{\P(R)}} = {\sum_{R\geq R(M)}{\P(R)}}
\end{equation}

	If, on the other hand, the cell were unoccupied, the original
distribution is hardly altered, except that the particular $R = R(M)$
is slightly less likely to occur, since one possible way to achieve it
is eliminated.  The cell in question is one of ``n'' cells we assumed
on the range surface, so we can say the chance of $R(M)$ happening is
reduced by the factor $(n-1)/n$.  The probabilities of other ranges
are not affected directly, so the result is a slight ``notching'' at
$R=R(M)$ of the a-priori $\P(R)$ distribution. Simply notching it,
however, reduces its total area, which must then be renormalized by
the proper factor to restore the area of the distribution to unity,
increasing all the other probabilities slightly:

\begin{equation} \label{rangen}
\p(M|\n{\o}) = {{\P(R(M))\times(n-1)/n}\over{1-\P(R(M))/n}} = 
\P(R(M))\times{{n - 1}\over{n - \P(R(M))}}
\end{equation}

\subsection{Interior Region}

	If we assume a perfect sensor, an occupied cell in the wedge
at a range less than $R(M)$ would always be detected and thus prevent
a the reading $M$.  So in this case

\begin{flushright}
	[interior region probabilities]
\end{flushright}

\begin{equation} \label{interiorp}
\p(M|\o)  =  0
\end{equation}

if the cell in the interior is unoccupied, then the range ``Rc'' at
which it occurs is less likely than other ranges.  Suppose there would
be $k$ cells occupied by a range arc at distance $Rc$.  By reasoning
like that of formula \ref{rangen}, the probability of $Rc$ would be
reduced by a factor of $(k-1)/k$, but the overall distribution would
have to be normalized by dividing by $(1 - \P(Rc)/k)$.  This
normalization raises the probability of $R(M)$ from its a-priori
value:

\begin{equation} \label{interiorn}
\p(M|\n{\o}) =  {{\P(R(M))}\over{1 - \P(Rc)/k}}
\end{equation}

\section{Measurement Errors}

	Of course our sensors are not perfect in the sense of the last
section. Sometimes they fail to respond to an occupied location, at
other times they give a spurious indication of occupancy.  It is easy
to modify the perfect case formulas for this.  Suppose there is a
small chance $e_{hit}$ that an empty cell will act as if it were
occupied.  Suppose also that there is a small possibility $e_{miss}$
that an occupied cell will fail to be detected.  We can then construct
the formulas with error from the error-free cases using the general
formulas:

\begin{equation} \label{errf1}
\p(M|\o)_{error}=\p(M|\o)\times(1-e_{miss})+\p(M|\n{\o})\times e_{miss}
\end{equation}

\begin{equation} \label{errf2}
\p(M|\n{\o})_{error}=\p(M|\n{\o})\times(1-e_{hit})+\p(M|\o)\times e_{hit}
\end{equation}

	If $e_{hit} = e_{miss} = 1/2$, then the sensors are returning
random readings. This is captured in (\ref{errf1}-\ref{errf2}) as
$\p(M|\o)_{error} = \p(M|\n{\o})_{error}$, the non informative case,
similar to formula (\ref{equalprob}).  As $e_{hit}$ and $e_{miss}$
grow closer to $1/2$, the reading becomes less and less informative.
If $e_{hit} = e_{miss} = 1$ the sensor is perfect, but the hit and
miss indications are mistakenly swapped.  In that case
(\ref{errf1}-\ref{errf2}) restores the correct pairing.

	A real sensor can be modeled, somewhat redundantly, by varying
$e_{hit}$ and $e_{miss}$ over the sensed area.  Towards the edges of
the beam they will creep towards $1/2$, achieving this value perfectly
outside the beam, and perhaps at the boundary of the interior and
range surface regions.

	If $0\leq e_{hit},e_{miss}\leq 1$, the error formulas can only
reduce or leave unchanged the amount of information provided by the
``perfect'' estimates of $\p(M|\o)$ and $\p(M|\n{\o})$.

	The quantities $e_{hit}$ and $e_{miss}$ must be initialized to
some reasonable values when a system begins operation. They can then
be adjusted by an iterative ``learning'' process.  Each time a map is
constructed from many sensor readings, the correlation of each cell in
each individual sensor profile with the area of the total map it
overlays is recorded. This correlation is averaged over many maps.  If
the map often reports a high occupancy probability in a location where
the sensor profile indicates occupancy, $e_{hit}$ will be low for that
position in the sensor profile, otherwise it will be high.  Similarly
if the map usually has low probabilities at a site indicated empty in
the sensor profile, $e_{miss}$ will be low, otherwise it will be high.

	The detection probability required to compute the function $S$
in equation \ref{profile} is given by the expression $1-e_{miss}$.  A
more accurate computation of $S$ would also take into account the
chance that an empty cell would falsely register as occupied. So a
good estimate for $S$ would be:
\begin{equation}\label{errffin}
S(R) = \sum_R\p(\o|A)\times (1-e_{miss}) + \p(\n{\o}|A)\times e_{hit}
\end{equation}

\section{Stereo Range Measurements}

	Another approach to incorporating individual sensor readings
uses the independent combining formula \ref{combine}.  In general this
will result in inferior maps, but it requires less computation.
Here's an example from a consideration of a stereo vision measurement.
One of our stereo approaches matches edges crossing a given scanline
in the left and right images.  A range is deduced from the relative
horizontal shift of each edge between the two images. The edges cannot
be located with infinite precision, so there is some uncertainty in
depth, given by a distribution $\P(D|m)$, the probability the object
ranged by stereo measurement $m$ is actually at distance $D$.  Note
that this is not the probability that location $d$ is occupied,
because $d$ may contain an object other than the edge being
ranged. Let $\p(d|m)$ represent the probability, given measurement
$m$, that $d$ is occupied by {\it any} object.  We can estimate
$\p(d|m)$ directly by the following reasoning.

	Consider the possible case that the ranged edge is at a
particular distance $D$ (this possibility has probability
$\P(D|m)$). In this case, and assuming a perfect sensor as we did in
the sonar example, we can conclude that
\begin{equation}\label{ster1}
\p(d|D) = 0\qquad when\qquad d<D
\end{equation}
since a perfect sensor would detect an intervening object
\begin{equation}\label{ster2}
\p(d|D) = 1\qquad when\qquad d=D
\end{equation}
since the object is at D
\begin{equation}\label{ster3}
\p(d|D) = \p(\o)\qquad when\qquad d>D
\end{equation}
since the edge blocks the view behind it, the measurement gives
us no special information beyond d=D.

Graphically:

\rule{0in}{2in}

	Relationship (\ref{ster1}-\ref{ster3}) holds only if the edge
is exactly at distance $D$.  Measurement $m$ tells us only that there
is a probability $\P(D|m)$ that this is the case.  To get the overall
probability, we must sum the $\p(d|D)$s over all possible $D$'s, each
weighted by its probability of actually being the case. Thus
\begin{equation}\label{sterfin}
\p(d|m) = \sum_D{(\p(d|D)\times\p(D|m))}
\end{equation}

	This will generally have the form

\rule{0in}{2in}

	Though if the spread of $\p(D|m)$ is very large the hump at
$d=m$ will be attenuated, and the curve will have the approximate
shape:

\rule{0in}{2in}

	The values of $\p(d|m)$ can be used directly to update maps by
use of the combining formula \ref{combine}.

\section{Acknowledgement}

	This work has been supported since 1981 by the Office of Naval
Research under contract N00014-81-K-503.  Alberto Elfes and Larry
Matthies made key contributions to the development of these ideas, and
I thank them warmly. I also wish to thank Peter Cheeseman for setting
my nose firmly in the Bayesian groove.

\section{Advertisement}

	We've recently discovered a general formulation that captures
and improves on the ideas of formulas \ref{sonex}, \ref{rangen},
\ref{interiorn}, \ref{errffin} and \ref{sterfin}.  It results in a
single procedure that can process a broad class of range readings, of
any dimensionality, at a cost linear in the volume of the sensor
footprint and quadratic in its volume of range uncertainty.  Look for
the forthcoming {\bf A Bayesian Method for Certainty Grids} by Hans
Moravec and Dong Woo Cho.

\section{References}
\begin{enumerate}
\item\label{Stewart88}
Stewart, W. K., {\bf A Model-Based Approach to 3-D Imaging and Mapping
Underwater}, Proceedings of the 7th International Conference and
Exhibit on Offshore Mechanics and Arctic Engineering (OMAE), Houston,
Texas, February 7-12, 1988.

\item\label{ElfMat87}
Matthies, L. H. and A. Elfes, {\bf Sensor Integration for Robot
Navigation: Combining Sonar and Stereo Range Data in a Grid-Based
Representation}, Proceedings of the 26th IEEE Decision and Control
Conference, Los Angeles, CA, December 9-11, 1987.

\item\label{Kweon87}
Kweon, I. S., personal communication, Robotics Institute,
Carnegie-Mellon University, Pittsburgh, PA, 15213, October 1987.

\item\label{Stewart87}
Stewart, W. K., {\bf A Non-Deterministic Approach to 3-D Modeling
Underwater}, 5th Symposium on Unmanned Untethered Submersible
Technology, University of New Hampshire, June, 1987.

\item\label{Serey86}
Serey, B. and L. H. Matthies, {\bf Obstacle Avoidance using 1-D Stereo
Vision}, CMU Robotics Institute Report, November, 1986.

\item\label{Elfes86}
Elfes, A. E, {\bf A Sonar-Based Mapping and Navigation System},
Workshop on Robotics, Oak Ridge National Lab, Oak Ridge, TN, August,
1985. (invited presentation), in the proceedings of the 1986 IEEE
International Conference on Robotics and Automation, San Francisco,
April 7-10 1986 also to appear as an invited paper in IEEE
Transactions on Robotics and Automation.

\item\label{Kadonoff86}
Kadonoff, M., F. Benayad-Cherif, A. Franklin, J. Maddox, L. Muller,
B. Sert and H. Moravec, {\bf Arbitration of Multiple Control
Strategies for Mobile Robots}, SPIE conference on Advances in
Intelligent Robotics Systems, Cambridge, Massachusetts, October 26-31,
1986. In SPIE Proceedings Vol 727, paper 727-10.

\item\label{Moravec85}
Moravec, H. P. and A. E. Elfes, {\bf High Resolution Maps from Wide
Angle Sonar}, proceeding of the 1985 IEEE International Conference on
Robotics and Automation, St. Louis, March, 1985, pp 116-121, and
proceedings of the 1985 ASME conference on Computers in Engineering,
Boston, August, 1985.

\item\label{Berger85}
Berger, J. O., {\bf Statistical Decision Theory and Bayesian
Analysis}, 1985.

\item\label{Thorpe84}
Thorpe, C. E., {\bf Path Relaxation: Path Planning for a Mobile
Robot}, CMU-RI-TR-84-5, Robotics Institute, Carnegie-Mellon
University, April, 1984.  also in proceedings of IEEE Oceans 84,
Washington, D.C., August, 1984 and Proceedings of AAAI-84, Austin,
Texas, August 6-10, 1984, pp. 318-321.

\item\label{Moravec80}
Moravec, H. P., {\bf Robot Rover Visual Navigation}, UMI Research
Press, Ann Arbor, Michigan, 1981. also available as {\bf Obstacle
Avoidance and Navigation in the Real World by a Seeing Robot Rover},
Stanford AIM-340, CS-80-813 and CMU-RI-TR-3.

\end{enumerate}
\end{document}