Monte Carlo Localization: New Project and Blog

[calamity]

I have a blog about my new project concerning robot Monte Carlo Localization. You can read my blog and the progress of my project at

http://lejosnxt.blogspot.com/

[calamity]

The beta code for my project has been released. You can find it here on my blog:

I hope that you will consider trying out my code, reporting any bugs to me.

http://lejosnxt.blogspot.com/2009/01/be ... eased.html

[lawrie]

Have you looked at the MCL classes in lejos.localization and the code I posted on this forum some time ago?

I see you run the particle filtering on the PC. The first version I wrote also did this but I now run the particle filtering on the NXT.

I have updated lejos.localization in SVN for the 0.8 release.

I will look at your implementation in more detail if I get time. I have also been sent MCL from from Professor David Cohen at London University . I have not studied it in detail yet, to see how it differs from my version in lejos.localization. His version is based on RobotC code that is available from www.itee.uq.edu.au/~wyeth/NXT/.

I am interested in adding support to leJOS for other probabalistic robotics algorithms.

[calamity]

Have you looked at the MCL classes in lejos.localization and the code I posted on this forum some time ago?

No, I did not use your code because I am doing a demo in which I need to explain how the algorithm works in some detail. With the randomization, weighting, and noise functions hidden from my view, I cannot explain the algorithm in the kind of detail that I would like.

However, I did do a mini-implementation of your code. I have comments below.

I see you run the particle filtering on the PC. The first version I wrote also did this but I now run the particle filtering on the NXT.

This choice is intentional because I want to show the students the results over a data projector connected to the PC. Transmitting the particle set from the nxt brick over bluetooth to the PC would be impractical. Instead, the brick is essentially a mobile sensor platform that obediently responds to the PC's movement and turn commands.

I think that it would be ideal if your code could run on the PC or brick as the user chooses.

I have updated lejos.localization in SVN for the 0.8 release.

I look forward to seeing it.

.I will look at your implementation in more detail if I get time. I have also been sent MCL from from Professor David Cohen at London University . I have not studied it in detail yet, to see how it differs from my version in lejos.localization. His version is based on RobotC code that is available from www.itee.uq.edu.au/~wyeth/NXT/.

I will look at Professor David Cohen's code.

I am interested in adding support to leJOS for other probabalistic robotics algorithms.

We share a common interest. I am interested in probabilistic robotics too.

I think that the code would be so much better if it added an optional interface such as

Code: Select all

public interface MCLfunction{

     ParticleSet initialize();

     float weightFunction();

     float noiseFunction();
}

In this way, the user could supply his own randomization function, weighting function for the particles, and the noise function for when the particles are moved or turned. If the interface is not implemented, the algorithm would use its default methods.

I think that the code would be far superior if BORDER were not a constant, but rather a private variable initialized to 20, but which has associated get and set methods. A border of 20 may not be appropriate depending upon what unit distance the user has in mind.

Finally, I had a few minor issues. The code defines a point. That's fine. The code would be more consistent if it then went on to define a line and a pose in terms of a point.

[lawrie]

You can look at the latest code in SVN at http://lejos.svn.sourceforge.net/viewvc ... alization/. I have tidied it up quite a bit including making border a variable.

I will look at your MCLFunction interface suggestion. I send the particle set back and forth to the PC and NXT and it does not take too long, and the algorithm is fairly slow in the NXT anyway (particularly resample). I have added methods in ParticleSet to dump to and load from a data stream to make this easier. I show a graphical representation of the progress of the algorithm on the PC. I think the current lejos.localization code will run on the PC or NXT. Currently you have to use classes.jar in a PC program to use it on the PC, but that will change in the 0.8 release.

One reason I did not use Point in Pose is that I want the Particle object to be as small as possible so that I can use as many particles as possible on the NXT. (I can currently support at least 300). Each object has an overhead so using Point would reduce the number of particles I can use.

I hope to improve the lejos support for probabilistic algoithms as I learn more about the subject. I am currently working my way through Sebastian Thrun's book: Probabistics Robotics.

I will send you David Cohen's Java code. The URL was to the RobotC implementation that he based his code on.

[calamity]

You can look at the latest code in SVN at http://lejos.svn.sourceforge.net/viewvc ... alization/. I have tidied it up quite a bit including making border a variable.

I am looking at it. Thank you.

I will look at your MCLFunction interface suggestion. I send the particle set back and forth to the PC and NXT and it does not take too long, and the algorithm is fairly slow in the NXT anyway (particularly resample).

I agree that the search can be slow, but, in looking at your code. I suspect that it's O(N^2). Quadratic time is quite slow. Please forgive me if I've misread your code, but there appears to be a for-loop running inside a while-loop.

I got my re-sampling to run in NlogN time. The N factor is due to our need to plod through all N samples of the new set. The logN factor is due to a binary search to find the particle that should be included in the sample set.

Code: Select all

   private void chooseNewPoseSet(){
      Pose [] oldPoses = displayPanel.getPose();
      Pose [] newPoses = new Pose[oldPoses.length];
      
      for(int k = 0; k < oldPoses.length; k++){
         double choice = Math.random();
         boolean notFound = true;
         int lower = 0;
         int upper = oldPoses.length - 1;
         int trialPose = 0;
         int count = 0;
         while(notFound){
            count++;
            if(count> 20){
               System.out.println("Too many times.");
               System.exit(0);
            }
            trialPose = (int)Math.floor(((upper - lower)/2.0)) + lower;
            //System.out.print("Trial:  ");
            //System.out.println(trialPose);
            if(choice <= oldPoses[trialPose].getCumWeight() &&
                  choice > (oldPoses[trialPose].getCumWeight()-
                        oldPoses[trialPose].getWeight())){
               notFound = false;
            }
            else if(choice > oldPoses[trialPose].getCumWeight()){
               lower = trialPose+1;
            }
            else
            {
               upper = trialPose-1;
            }
            if(lower == upper || upper < lower) break;
            
         }
         //newPoses[k] = new Pose(oldPoses[trialPose].getHeading(),
         //      oldPoses[trialPose].getX(),oldPoses[trialPose].getY());
         int noise = -2 + (int)Math.random()+4;
         newPoses[k] = new Pose(oldPoses[trialPose].getHeading() + noise,
               oldPoses[trialPose].getPoint().getX()+noise,
               oldPoses[trialPose].getPoint().getY()+noise);
      }
      
      System.arraycopy(newPoses, 0, oldPoses, 0, newPoses.length);
   }



The for-loop runs N times, but the while loop is certain to find the sample in no more than logN iterations because it is conducting a binary search for it.

Please note that my noise function is just a lot of nonsense and I need to change it to the Box-Muller algorithm. I just put something in until I could find something better.

I have added methods in ParticleSet to dump to and load from a data stream to make this easier. I show a graphical representation of the progress of the algorithm on the PC. I think the current lejos.localization code will run on the PC or NXT. Currently you have to use classes.jar in a PC program to use it on the PC, but that will change in the 0.8 release.

One reason I did not use Point in Pose is that I want the Particle object to be as small as possible so that I can use as many particles as possible on the NXT. (I can currently support at least 300). Each object has an overhead so using Point would reduce the number of particles I can use.

I hope to improve the lejos support for probabilistic algoithms as I learn more about the subject. I am currently working my way through Sebastian Thrun's book: Probabistics Robotics.

I have that book too.

I will send you David Cohen's Java code. The URL was to the RobotC implementation that he based his code on.

Thank you. I look forward to it.

[lawrie]

If you give me your email address of send it to lawrie.griffiths@ntlworld.com, I will send you David Cohen's code.

I don't know if my version of resample is O(n2) or O(nlog(n)). The basic algorithm comes from the java code associated with the book: Artificial Intelligence: A Modern Approach. I will do some performance tests sometime and also try your algorithm and Gordon Wyeth's, if I can convert them to work with my localization classes.

[lawrie]

I tried your beta code and the FileDump class seems to be missing.