sonar certainty grid algorithm?

looking for the code (or an equation with good documentation)
PROBABILITY OF OBSTRUCTION: this returns the probability that cell X,Y (which is a cell
that the arc of the sonar beam intersects) is occupied input:angle of sonar input:sonar echo distance input:x,y of cell in certainty grid which sonar arc intersects * OUTPUT:probability that cell is occupied
also: PROBABILITY OF NON-OBSTRUCTION: this returns the probability that cell x,y (which is a cell that in the area of the sonar beam, NOT the arc) input:angle of sonar input:sonar echo distance input:x,y of cell in certainty grid within sonar beam
OUTPUT:probability that cell is empty.
at: http://real.uwaterloo.ca/~robot/sonar/sonar_9.html
I find the equation (for probability that NON arc cells within sonar beam are empty):
begin quote: " For the point P = (x, y, z) belonging to the volume swept by the sonar beam, the following have been defined:
R range measurement returned by the sonar sensor
e maximum sonar measurement error
w sensor beamwidth
W solid angle subtending the main lobe of the sensitivity function
S = (xs, ys, zs), position of the sonar sensor
d distance from P to S
q angle between the main axis of the beam and P as seen from S
The empty probability density function, PE, for a point P inside the sonar beam is given by Equation 6.
PE(x,y,z) = Er(d ) * E(q ) " end quote
I understand q,d,S,R,e, and w.
I cannot figure out what W is "W solid angle subtending the main lobe of the sensitivity function "
can someone explain this?
is this the angle that the sonar is pointing in?
the equation goes on:
begin quote (I had to do some text tricks to get the graphics of the equation look right) " Er(distanceToCell) = 1 -( distanceToCell - Rmin )^2 ( -------------------- ) ( R-maxerror-Rmin )
if distanceToCell..E.. [Rmin,R-maxerror] = 0
Ea(angleToCell) = 1 - ( 2*angleToCell )^2 ( ---------------- ) ( W ) for
angleToCell..E.. [ -W W ] [ ---,--- ] [ 2 2 ] " .E.. = symbol I couldn't mimic with ascii text
end quote
what is Rmin? W? these are the only variables I cannot understand. Rmin is not mentioned in the list of variables, and W seems to have an obscure description.
can someone give easy to understand PROOF for the above equation? I can at least compute Ea and Er..
I don't understand the IF or the FOR in the equation.
can you explain to me the IF and FOR parts of the equation?
ps I am working with high-school algebra and geometry, and a mathematical mind.
I can compute the coordinates of all cells within the sonar beam, determine which cells the arc intersects, and the cells between the sensor and the arc which should be unobstructed.
now, how to equate probability, given distance (or echo time)?
next would be:
how to add (this processed sonar data) to (previous certainty grid cell values)
later,
Rich aiiadict AT hotmail DOT com http://rich12345.tripod.com
************** comp.robotics.research (moderated) ************** Summary: Academic, government & industry research in robotics. Archives and information: http://www.metamech.com/crr Charter: http://www.metamech.com/crr/Charter Meta-discussions/information: snipped-for-privacy@metamech.com
Add pictures here
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
Add image file
Upload

Polytechforum.com is a website by engineers for engineers. It is not affiliated with any of manufacturers or vendors discussed here. All logos and trade names are the property of their respective owners.