Slam Loop Closure Robotics

Hi,

Firstly thanks for your time.

I'm working on a two wheeled robot SLAM project in Matlab and am having trouble with the loop closure stage.

The dropbox files linked below are the Matlab code and the data file it uses. The data has been created from a simulated robot which travels around a square.

On the output simulation from the Matlab code, the green line is the robots path and the red dots are ultrasonic sensor measurements. As you will be able to see, the line is not a loop at the moment and this is my problem.

The mathematical basis of the code is from the following document (section 3).

http://www.tinyphoon.com/rainbow/_tinyphoon/Documents/SimplePathPlanning.pdf

Code Variables: LeftMotor and RightMotor are the angular velocities of each wheel. Time is the timestamp associated with each measurement.

Matlab Code - https://www.dropbox.com/s/wodoytlejz46dfm/PathPlan.m

Data File - https://www.dropbox.com/s/z0bgc7im937hklq/18thApril2.mat

Any help would be much appreciated.

Thanks, Steve