jtrackerguimote.java

该java代码给出了粒子滤波器的编程仿真,有一个仿真的界面

/****************************************************************/
/*                      JTrackerGUI	                            */
/*                                                              */
/****************************************************************/
import java.awt.*;
import java.awt.event.*;
import javax.swing.*;
import java.util.Random;
import java.lang.Math;
import java.text.DecimalFormat;
import javax.swing.BorderFactory;
import javax.swing.border.Border;
import javax.swing.border.EtchedBorder;
import java.util.StringTokenizer;

/**
 * JTracker Applet
 *
 * Created by:
 * Nicholas Rancourt, Timothy Lindblom, Ly Luu, and Daniel Peterson
 *
 * Advisor: Professor Antonia Papandreou-Suppappola
 *
 * Co-advisors: Amit Chhetri
 *
 *------------------------------------------------------------------
 * This program simulates target tracking using a particle filter.
 *------------------------------------------------------------------
 */
public class JTrackerGUIMote extends JApplet implements Runnable
{
	// Variables declaration
	public JPanel contentPane;
	//-----
	public JLabel Seperator;
	public JTextField Yvel;
	public JTextField Xin;
	public JTextField SensError;
	public JTextField Vel;
	public JTextField Xsensor;
	public JTextField Ysensor;
	public JTextField MCIter;
//	private JPanel lp;
	public JButton startButton;
	public JLabel jtracklabel;
	public JLabel paralabel;
	public JLabel ylabel;
	public JLabel xlabel;
	public JLabel xlabel22;
	public JLabel xlabel2;
	public JLabel pathLabel;
	public JLabel measLabel;
	public JTextField Qin;
	public JLabel legendlabel;
	public JLabel legend;
	public JTextArea helpinfo;



	public StringTokenizer tokenizer;
	public String token;
	public boolean success;
	public Object dialogInput;
	public double xact, yact, ract, rdotact, angleact, xin, yin, rin, rdotin, anglein;


		//-----
//	private JPanel rp;
		//-----
	private EPanel pathErrorPanel;
		//-----
	private EPanel measErrorPanel;
	//-----

	public int frame, delay, appletWidth = 600, appletHeight = 400, length=50, numParts=2000, niter=5, MC, deltaT;
	private int NumSens = 5;
	private double r = 0.5;
	private double sqrtR = Math.sqrt(r);
	public Thread animator;
	public double[][] path, xhat, error;
	public double[] xmeas;
	public double[] TrueMeas, h_x, e_x, distall;
	public double[][] noise={{0.45,0.3617,0.9323},{1.4468,0.7572,1.1944},{2.7969,1.0852,1.35},{4.4138,1.3606,1.4659}};
	public Random generator;
	public double xmax, ymax, xmin, ymin, xvelocity, yvelocity, xinit, yinit, Qval, wtotal, xsensor, ysensor, fps;
	private double phiStdvRad = Math.PI/180, rStdv = 10, rDotStdv = 10;
	private double phiVar, rVar, rdotVar;
	private double So = 100;
	private double epsilon = Math.pow(10,-10);
	private double b = 3.2368;
	private double iXPos, iYPos, iXVel, iYVel;
	private double[] IMean;
	private double[][] SLoc;
	private int[][] SLocPixel;
	public Image jet, radar;
	public LeftPanel lp;
	public RightPanel rp;
	public CheckPanel cp;
	public boolean run, animating, ploterror, paused, walk;
	private particleList parts, resampled;
	private MediaTracker mediaTracker;
	private JSeparator divLine0;
	private DecimalFormat dFormat;

	public double sensorXmeter, sensorYmeter, sensor2Xmeter, sensor2Ymeter, sensor3Xmeter, sensor3Ymeter, sensor4Xmeter, sensor4Ymeter, sensor5Xmeter, sensor5Ymeter, targetXmeter, targetYmeter;
    public int sensorX, sensorY, sensorWidth, sensorHeight, targetX, targetY, targetWidth, targetHeight;


	public void init()
	{

// Compute sensor variances from the standard deviations defined above.
		phiVar = Math.pow(phiStdvRad,2);
		rVar = Math.pow(rStdv,2);
		rdotVar = Math.pow(rDotStdv,2);

		// The Boolean run is used to determine if the algorithm has been run
		// since the applet was started.  This is used for intial display
		// purposes.
		run=false;
		ploterror=false;
		animating = false;
		walk = false;

		dFormat = new DecimalFormat("0.#");

		// lp, a LeftPanel (defined below) contains the inputs.
		lp = new LeftPanel();
		cp = new CheckPanel();

		// Load the images of the jet and radar
		jet=getImage(getCodeBase(), "walk2.gif");
		radar=getImage(getCodeBase(), "mic.gif");

		// MediaTracker is used to wait until the images are loaded before continuing.
		MediaTracker mediaTracker = new MediaTracker(this);
		mediaTracker.addImage(jet, 0);
		mediaTracker.addImage(radar, 1);
		try
		{
			mediaTracker.waitForID(0);
			mediaTracker.waitForID(1);
		}
		catch (InterruptedException ie)
		{
			System.err.println(ie);
			System.exit(1);
		}


		// rp, a RightPanel (defined below) contains the graph showing the target track.
		rp = new RightPanel();

		// Create Layout with lp on left and rp on right.
	//	getContentPane().add(new JSplitPane(JSplitPane.HORIZONTAL_SPLIT, lp, rp));
		setSize(appletWidth, appletHeight);

		// The frame variable keeps track of the time step currently executing.
		frame = 0;



		// Used by the thread.
		delay = (fps > 0) ? (int)(1000 / fps) : 100;



		// Initialize the particleLists for the particles before and after resampling.
		parts=new particleList(numParts);
		resampled=new particleList(numParts);

		// Create a random number generator to add random noise.
		generator = new Random();

		// Display the images and axes

		contentPane = (JPanel)this.getContentPane();
		//-----
		Seperator = new JLabel();
		Yvel = new JTextField();
		Xin = new JTextField();
		SensError = new JTextField();
		Vel = new JTextField();
		Xsensor = new JTextField();
		Ysensor = new JTextField();
		MCIter = new JTextField();
		Qin = new JTextField();
	//	lp = new JPanel();
		//-----
		jtracklabel = new JLabel();
		paralabel = new JLabel();
		ylabel = new JLabel();
		xlabel = new JLabel();
		xlabel22 = new JLabel("Time step");
		xlabel2 = new JLabel("Time step");
		pathLabel = new JLabel("Estimation error in meters");
		measLabel = new JLabel("Measurement error in meters");
		legendlabel = new JLabel();
		legend = new JLabel();
		helpinfo = new JTextArea();
		helpinfo.setBorder(BorderFactory.createEtchedBorder(EtchedBorder.LOWERED));

		SLoc = new double[2][5];
		SLoc[0][0] = 0;
		SLoc[1][0] = 8;
		SLoc[0][1] = -2;
		SLoc[1][1] = 0;
		SLoc[0][2] = 10;
		SLoc[1][2] = 13;
		SLoc[0][3] = -7;
		SLoc[1][3] = -7;
		SLoc[0][4] = 12;
		SLoc[1][4] = 0;
		TrueMeas = new double[NumSens];
		h_x = new double[NumSens];
		e_x = new double[NumSens];


		contentPane = (JPanel)this.getContentPane();
		//-----
	//	rp = new JPanel();
		//-----
		pathErrorPanel = new EPanel();
//		pathErrorPanel.setToolTipText( "Distance between the actual position of the target and the estimated \n position in meters.  Averaged after each MC iteration.");
		pathErrorPanel.addMouseListener(new pathmouse());
		//-----
		measErrorPanel = new EPanel();
//		measErrorPanel.setToolTipText( "Distance between the actual position of the target and the measurement of the position in meters.  Averaged after each MC iteration.");
		measErrorPanel.addMouseListener(new measmouse());
		//-----
		startButton = new JButton();
		//
		// jtracklabel
		//
		Icon jtrack = new ImageIcon( getClass().getResource("Jtracker.JPG"));
		jtracklabel.setIcon(jtrack);
		//
		// paralabel
		//
		Icon para = new ImageIcon( getClass().getResource("parameters3.JPG"));
		paralabel.setIcon(para);
		//
		// ylabel
		//
		Icon ybar = new ImageIcon( getClass().getResource("ybar.JPG"));
		ylabel.setIcon(ybar);
		//
		// xlabel
		//
		Icon xbar = new ImageIcon( getClass().getResource("xbar.JPG"));
		xlabel.setIcon(xbar);
		//
		// xlabel22
		//
		//Icon xbar22 = new ImageIcon( getClass().getResource("xbar2.JPG"));
		//xlabel22.setIcon(xbar22);
		//
		// xlabel2
		//
		//Icon xbar2 = new ImageIcon( getClass().getResource("xbar2.JPG"));
		//xlabel2.setIcon(xbar2);

		Icon legend12 = new ImageIcon( getClass().getResource("legend12.JPG"));
		legendlabel.setIcon(legend12);

		Icon legend13 = new ImageIcon( getClass().getResource("legend13.JPG"));
		legend.setIcon(legend13);
		divLine0 = new JSeparator();

		helpinfo.setBackground(new Color(227, 228, 230));
		helpinfo.setForeground(new Color(1, 1, 1));
		helpinfo.setOpaque(true);

		rp.setBorder(BorderFactory.createEtchedBorder(EtchedBorder.LOWERED));
		pathErrorPanel.setBorder(BorderFactory.createEtchedBorder(EtchedBorder.LOWERED));
		measErrorPanel.setBorder(BorderFactory.createEtchedBorder(EtchedBorder.LOWERED));


// contentPane
		//
		contentPane.setLayout(null);
		contentPane.setBackground(new Color(203, 221, 200));
		addComponent(contentPane, jtracklabel, 0,0,230,100);
		addComponent(contentPane, paralabel, 0,101,120,332);
		addComponent(contentPane, ylabel, 250,-1,40,321);
		//addComponent(contentPane, xlabel, 245,317,455,8);
		addComponent(contentPane, rp, 290,0,500,320);
		addComponent(contentPane, pathLabel, 520,325,160,14);
		addComponent(contentPane, pathErrorPanel, 400,340,390,60);
		addComponent(contentPane, xlabel2, 560,400,80,14);
		addComponent(contentPane, divLine0, 400, 414, 390, 3);
		addComponent(contentPane, measLabel, 520,415,170,14);
		addComponent(contentPane, measErrorPanel, 400,430,390,60);
		addComponent(contentPane, xlabel22, 560,490,80,14);
		addComponent(contentPane, helpinfo, 250,513,540,45);
		addComponent(contentPane, legendlabel, 0,435,170,120);
		addComponent(contentPane, legend, 100,510,180,50);
		addComponent(contentPane, cp, 235, 325, 160, 185);

		//
		// rp
		//
		rp.setLayout(new FlowLayout(FlowLayout.CENTER, 5, 5));
		rp.setBackground(new Color(6, 226, 252));
		//
		// pathErrorPanel
		//
		pathErrorPanel.setLayout(new FlowLayout(FlowLayout.CENTER, 5, 5));
		pathErrorPanel.setBackground(new Color(6, 226, 252));
		//
		// measErrorPanel
		//
		measErrorPanel.setLayout(new FlowLayout(FlowLayout.CENTER, 5, 5));

		//
		// contentPane
		//
		contentPane.setLayout(null);
		contentPane.setBackground(new Color(203, 221, 200));
		addComponent(contentPane, lp, 120,101,110,414);
		//

		this.setLocation(new Point(3, -6));
		this.setSize(new Dimension(800, 560));



		rp.repaint();
	}  /////////end of inti class
// This method is called when the "Start" button is pressed.  It starts the thread that
	// enables the animation.
	public void startThread()
	{
		animator=new Thread(this);
		animator.start();
		animating=true;
		run = true;
	}
	private class pathmouse implements MouseListener
	{

			public void mousePressed(MouseEvent e) {}
			public void mouseReleased(MouseEvent e) {}
			public void mouseClicked(MouseEvent e) {}
			public void mouseEntered(MouseEvent e)
			{
				helpinfo.setLineWrap(true);
				helpinfo.setText("Distance between the actual position of the target and the estimated position of the target in\n meters.  Averaged after each MC iteration.");
			}
			public void mouseExited(MouseEvent e) {}

	}
	private class measmouse implements MouseListener
	{

			public void mousePressed(MouseEvent e) {}
			public void mouseReleased(MouseEvent e) {}
			public void mouseClicked(MouseEvent e) {}
			public void mouseEntered(MouseEvent e)
			{
				helpinfo.setLineWrap(true);
				helpinfo.setText("Distance between the actual position of the target and the measurement of the position in\n meters.  Averaged after each MC iteration.");
			}
			public void mouseExited(MouseEvent e) {}

	}

//This method is called by the thread that was created in
	//the startThread method. It does the main animation.
	public void run()
	{
		// Remember the starting time
		long tm = System.currentTimeMillis();
		while (Thread.currentThread() == animator)
		{
		    // Delay depending on how far we are behind.
			try
			{
				tm += delay;
				Thread.sleep(Math.max(0, tm - System.currentTimeMillis()));
			}
			catch (InterruptedException e)
			{
				break;
			}

			// Advance the frame number and display the frame.
			if (frame<length-1)
			{
				frame++;
				rp.repaint();
			}
			// If the last frame is reached, stop animation.
			else
			{
				MC=MC-1;


				for (int count=0; count<length; count++)
				{
					double newError =Math.sqrt(Math.pow((path[0][count]-xhat[0][count]),2)+Math.pow((path[1][count]-xhat[1][count]),2));
					pathErrorPanel.error[count]=(newError+(niter-MC-1)*pathErrorPanel.error[count])/(niter-MC);
				}
				ploterror=true;
				pathErrorPanel.repaint();
				measErrorPanel.repaint();


				if (MC==0)
				{
					animator = null;
					animating = false;
					targetXmeter = path[0][length-1];
					targetYmeter = path[1][length-1];
				}

				else
				{
					frame=0;
					animator = null;
					animating = false;

					if(cp.pauseBox.isSelected())
						paused = true;