The DARPA LAGR program has the challenging aim of producing a robot that can autonomously navigate in varied outdoor environments from tall grass fields to heavily wooded terrain. The robot platform on which this task is to be accomplished has two stereo pairs, which are its only means of perception.
My primary task in the project was the creation of a 6-DOF pose estimation algorithm that samples various measurement streams, such as GPS, IMU, and visual odometry, and computes an optimal estimate of the robot pose. Since the frequency and latency of the measurement streams differ, the estimate is computed using fixed-lag smoothing. Fast linearization and matrix factorization methods are used to enable the filter to run at upto 40 Hz, making it more than adequate for real-time use. The pose estimator has clocked hundreds of hours of testing on the robot.
I also worked on a grid-mapping algorithm using depth measurements from stereo.