landmarksampleupdater.java

利用JAVA编写的群体机器人局部通讯完成一定得队形控制

/** LandmarkSampleUpdate.java
 * heavily borrowed from scott lenser's code.
 */
/* This code is part of the localize package of TeamBots.
 * Copyright (c) 1999, 2000 by John Sweeney and Carnegie Mellon University
 */
package EDU.cmu.cs.coral.localize;
import java.lang.Math;
import EDU.gatech.cc.is.util.Vec2;


public class LandmarkSampleUpdater extends GaussianEvaluator {
    //the marker's location
    protected Vec2 location;

    public LandmarkSampleUpdater() {
    
	location = new Vec2();
    }
	
    
    public LandmarkSampleUpdater(int nv) {
	super(nv);
       
	location = new Vec2();
    }
    
    public void setLocation(Vec2 loc) {
	setLocation(loc.x, loc.y);
    }

    public void setLocation(double x, double y) {
	location.setx(x);
	location.sety(y);
    }

    public void updateSample(Sample s) {
	double dist;
	double allo_angle;
	double ego_angle;
	double x = s.data[Sample.x];
	double y = s.data[Sample.y];
	double theta = s.data[Sample.t];

	double [] in = new double[2];
	double [] out;

	//distance from sample to landmark
	dist = Math.sqrt( ((location.x - x)*(location.x - x)) +
		     ((location.y - y)*(location.y - y)));

	//angle between the two positions
	allo_angle = Math.atan2(location.y-y, location.x-x);
	
	if (allo_angle < 0) {
	  allo_angle = -allo_angle;
	}
	
	if (theta < 0)
	  ego_angle = allo_angle +theta;
	else
	  ego_angle = allo_angle - theta;
	
	//ego_angle = allo_angle - theta;
	
	while (ego_angle < -Math.PI)
	    ego_angle += 2.0*Math.PI;

	while (ego_angle >= Math.PI)
	    ego_angle -= 2.0*Math.PI;
	/*   
	if (theta < 0) theta = -theta;
	ego_angle = theta+allo_angle;
	System.out.println("ego_angle = "+ego_angle);
	*/
	in[0]=dist;
	in[1]=ego_angle;

	//		System.out.println("LSU: update: dist = "+dist+" ego_angle = "+ego_angle+" distMean = "+mean[0]+" angleMean = "+mean[1]);
	
	out = evaluate(in);
	//		System.out.println("LSU: out[0] = "+out[0] + " out[1] = "+out[1]);
	s.data[Sample.w] *= out[0];
	s.data[Sample.w] *= out[1];
    }
  
  public double updateSample(double x, double y, double theta, double weight) {
    double dist;
    double allo_angle;
    double ego_angle;

    double [] in = new double[2];
    double [] out;
    
    //distance from sample to landmark
    dist = Math.sqrt( ((location.x - x)*(location.x - x)) +
		      ((location.y - y)*(location.y - y)));
    
    //angle between the two positions
    allo_angle = Math.atan2(location.y-y, location.x-x);
    
    if (allo_angle < 0) {
      allo_angle = -allo_angle;
    }
    
    if (theta < 0)
      ego_angle = allo_angle +theta;
    else
      ego_angle = allo_angle - theta;
    
    //ego_angle = allo_angle - theta;
    
    while (ego_angle < -Math.PI)
      ego_angle += 2.0*Math.PI;
    
    while (ego_angle >= Math.PI)
      ego_angle -= 2.0*Math.PI;
    /*   
	 if (theta < 0) theta = -theta;
	 ego_angle = theta+allo_angle;
	 System.out.println("ego_angle = "+ego_angle);
    */
    in[0]=dist;
    in[1]=ego_angle;
    
    //		System.out.println("LSU: update: dist = "+dist+" ego_angle = "+ego_angle+" distMean = "+mean[0]+" angleMean = "+mean[1]);
    
    out = evaluate(in);
    //		System.out.println("LSU: out[0] = "+out[0] + " out[1] = "+out[1]);
    weight *= out[0];
    weight *= out[1];
    
    return weight;
  }
  
}