SHARE
TWEET

Henrik Klaus Martin

a guest Jan 20th, 2009 130 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. public class MCL {
  2.         int samplescount=0;
  3.         Sample[] samples;
  4.         double xrange = 0.0;
  5.        
  6.         public MCL(int samplescount, double xrange){
  7.                 this.samplescount = samplescount;
  8.                 this.samples = new Sample[samplescount];
  9.                 this.xrange = xrange;
  10.                 for(int i=0; i<samplescount; i++)
  11.                         samples[i] = new Sample();
  12.         }
  13.        
  14.         public void initialise(){
  15.                 for(int i=0; i<samplescount; i++){
  16.                         samples[i].setX(Math.random()*xrange);
  17.                         samples[i].setImportance(1.0/samplescount);
  18.                 }
  19.         }
  20.        
  21.         public double Pmap(double x) {
  22.                 //return (x/xrange)*100.0;
  23.                 //return 0.0885 * (x/xrange)*255 + 33.439;
  24.          
  25.                 double[] a = {1.0,1,1,2,1,1,1,0,1,2,0,1,1,1,1,1,1,1,1,0,1,1,0,0,0};
  26.                 double[] b = {33.0,33,33,30,34,34,34,41,33,24,44,33,33,33,33,33,33,33,33,52,32,32,54,54,54};
  27.  
  28.                 for(int i=0; i<25; i++){
  29.                         if(x>=i && x<i+1){return a[i]*x+b[i];}
  30.                 }
  31.                 return 0.0;
  32.         }
  33.        
  34.         public double expPosition(){
  35.                 double sumX=0.0;
  36.                 for(int i=0; i<samplescount; i++){
  37.                         sumX += samples[i].getX();
  38.                 }
  39.                 return sumX/samplescount;
  40.         }
  41.        
  42.         public void printSamples(){
  43.                 for(int i=0; i<samplescount; i++){
  44.                         System.out.println(samples[i].getX() + " " + samples[i].getImportance());
  45.                 }
  46.         }
  47.        
  48.         public void update(double deltax, double sensor, double uncertainty){
  49.                 Sample[] newsamples = new Sample[samplescount];
  50.                 double sumimp=0.0;
  51.                
  52.                 for(int i=0; i<samplescount; i++){
  53.                         int k = chooseSample();
  54.                         double newx = samples[k].getX() + deltax + (Math.random()*uncertainty-uncertainty/2);
  55.                        
  56.                         if(newx > xrange){newx = xrange;}
  57.                         if(newx < 0.0){newx = 0.0;}
  58.                        
  59.                         double expectedsensor = Pmap(newx);
  60.                         double newimportance = Math.pow((100.0-Math.abs(sensor - expectedsensor))/100.0, 2);
  61.                         newsamples[i] = new Sample(newx, newimportance);
  62.                         sumimp += newimportance;
  63.                 }
  64.                
  65.                 for(int i=0; i<samplescount; i++){
  66.                         samples[i].setX(newsamples[i].getX());
  67.                         samples[i].setImportance(newsamples[i].getImportance()/sumimp);
  68.                 }
  69.         }
  70.        
  71.         public int chooseSample(){
  72.                 double r = Math.random();
  73.                 double sum=0; int count=0;
  74.                
  75.                 for(int i=0; i<samplescount; i++){
  76.                         sum += samples[i].getImportance();
  77.                         if(sum >= r){count = i; break;}
  78.                 }
  79.                
  80.                 return count;
  81.         }
  82. }
RAW Paste Data
Want to get better at Java?
Learn to code Java in 2017
Pastebin PRO Summer Special!
Get 40% OFF on Pastebin PRO accounts!
Top