Kalman Filter Help SLAM

Hi and thanks for your time,

I'm working on a two wheeled robot SLAM project in Matlab and need to implement a Kalman filter to my code. I am struggling to learn how to do this so was wondering if anyone could help.

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.

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

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

On the output simulation from the Matlab code, the green line is the robots path and the red dots are ultrasonic sensor measurements of objects.

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

To distinguish between which landmarks and ones already measured a kalman filter is needed.

I would be very grateful if anyone could lend a hand on this subject.

Thanks,

Steve