Predicting the Fiducials

(Amin's section)

One element necessary to run the Kalman Filter algorithm is a prediction on what fiducials should be seen from a given position. This can be done using the properties of the projection into the omnicam.

The angle relative to the horizontal base of the omnicam.
The angle of the pixel relative to the center of the omnicam (looking down at the omnicam)
The distance from the focus at which the line of sight intersects the paraboloid
The x position of the pixel in the image
The y position of the pixel in the image
Given the position of the robot and the known layout of the fiducials in the environment, an image can be generated of where the robot should see the fidicuals in the omnicam from that position. Each fiducial has a known (x,y,z) location in the world. Given a robot position, the fiducial images are rotated and translated to be robot-centric. Next, using the above equations, the (x,y) locations of the fiducials in the omni image can be calculated, as well as the angle from the center. These expected positions are used in the next step to help optimize using the Kalman Filter.

Predicted fiducials at (450, 30, 0)
No range was set so the position of all the fiducials is calculated. In use, only fiducials within a range would be considered.

Kalman Filter

    The Algorithm
  1. Move robot
  2. Update X by odometry
  3. Update P by error model of odometry
  4. Get image from omnicam
  5. Find fiducials in image
  6. Use Least Squares to find x which minimizes where µ is the current estimated position, E is the expectation function from the image and z(x) is the prediction fiducial positions at point x.
  7. Repeat
When the robot is moved forward, it takes an image with the omnicam. The fiducial-finding function is called which returns a expectation value at each angle. The Kalman Filter algorithm begins with an initial estimate of it's position and a covariance matrix based on the error model of the odometry. The fiducial-prediction algorithm is called to find the angles at which fiducials should be seen at the point. The sum of the energies at these angles in the fiducial-finding expectation function is found. Least Squares is used to iterate and find the x which minimizes this expectation value. The optimal x should be the corrected robot position.

Results

For the experiment, fiducials were placed on the walls in a hallway in the MARC building. They are placed on average 2 meters appart and are staggered between the left and right walls. Ideally, the robot will be able to see 2 or 3 fiducials clearly at any time. The positions of these fiducials is recorded.

A robot is then moved down the hallway, turn around, and moved back to the original starting point. The robot moves in a "meandering" motion (not straight). Images are recorded as the robot moves.

For our experiments, analysis of the images occurs offline but the system could easily be implemented and modifying to run online during robot movement.

Odometry-based localization. This is what the robot odometry reported as the robot position over time. It can be seen how error accumulates, especially angular error. This is most significant when the robot turns around at the end.
Unfortunately, we were unable to get the complete system working. The system ran, but results were not accurate. Least Squares was unable to find a better match than the initial guess in each case. There are two sources of error.

  1. Experiment Error - For this project, we gathering all the data to be processed offline. Images were recorded with on a DV recorder while the odometry was recorded by the robot. A source of error could be aligning the odometry readings with the correct image. If this alignment is off, the algorithm will be using incorrect odometry readings. Ideally, the algorithm should be able to correct for this error. If this alogrithm was implemented for actually use online, readings and images could be processed as they come in and this would not be a problem.
  2. Implementation Error - A significant source of the problem is in the implementation of the alogrithm. The Matlab Least Squares function was not behaving as expected. It was not properly searching the space of potential x.
Back to main page
Last modified: Thu Dec 6 11:02:26 EST 2001