jtrackerguimote.txt

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

			{
				try
				{
					yin=Double.parseDouble(token);
				}
				catch (NumberFormatException exception)
				{
					dialogInput = JOptionPane.showInputDialog(null,"Please reenter Sensor 1 y position.","Invalid y position",JOptionPane.ERROR_MESSAGE, null, null, token);
					try{token = dialogInput.toString();}
					catch(NullPointerException e){}
					continue;
				}
				success=true;
			}
			SLoc[0][0] = xin;
			SLoc[1][0] = yin;
			lp.Sensors[0].setText(Double.toString(xin)+", "+Double.toString(yin));

			//----------------------------------------------------
			//Get Sensor 2 position
			//----------------------------------------------------
			tokenizer = new StringTokenizer(lp.Sensors[1].getText(),"(), ", false);

			token = (tokenizer.hasMoreTokens()) ? tokenizer.nextToken() : "x position";

			success = false;
			while(!success)
			{
				try
				{
					xin=Double.parseDouble(token);
				}
				catch (NumberFormatException exception)
				{
					dialogInput = JOptionPane.showInputDialog(null,"Please reenter Sensor 2 x position.","Invalid x position",JOptionPane.ERROR_MESSAGE, null, null, token);
					try{token = dialogInput.toString();}
					catch(NullPointerException e){}
					continue;
				}
				success=true;
			}

			token = (tokenizer.hasMoreTokens()) ? tokenizer.nextToken() : "y position";

			success = false;
			while(!success)
			{
				try
				{
					yin=Double.parseDouble(token);
				}
				catch (NumberFormatException exception)
				{
					dialogInput = JOptionPane.showInputDialog(null,"Please reenter Sensor 2 y position.","Invalid y position",JOptionPane.ERROR_MESSAGE, null, null, token);
					try{token = dialogInput.toString();}
					catch(NullPointerException e){}
					continue;
				}
				success=true;
			}
			SLoc[0][1] = xin;
			SLoc[1][1] = yin;
			lp.Sensors[1].setText(Double.toString(xin)+", "+Double.toString(yin));

			//----------------------------------------------------
			//Get Sensor 3 position
			//----------------------------------------------------
			tokenizer = new StringTokenizer(lp.Sensors[2].getText(),"(), ", false);

			token = (tokenizer.hasMoreTokens()) ? tokenizer.nextToken() : "x position";

			success = false;
			while(!success)
			{
				try
				{
					xin=Double.parseDouble(token);
				}
				catch (NumberFormatException exception)
				{
					dialogInput = JOptionPane.showInputDialog(null,"Please reenter Sensor 3 x position.","Invalid x position",JOptionPane.ERROR_MESSAGE, null, null, token);
					try{token = dialogInput.toString();}
					catch(NullPointerException e){}
					continue;
				}
				success=true;
			}

			token = (tokenizer.hasMoreTokens()) ? tokenizer.nextToken() : "y position";

			success = false;
			while(!success)
			{
				try
				{
					yin=Double.parseDouble(token);
				}
				catch (NumberFormatException exception)
				{
					dialogInput = JOptionPane.showInputDialog(null,"Please reenter Sensor 3 y position.","Invalid y position",JOptionPane.ERROR_MESSAGE, null, null, token);
					try{token = dialogInput.toString();}
					catch(NullPointerException e){}
					continue;
				}
				success=true;
			}
			SLoc[0][2] = xin;
			SLoc[1][2] = yin;
			lp.Sensors[2].setText(Double.toString(xin)+", "+Double.toString(yin));

			//----------------------------------------------------
			//Get Sensor 4 position
			//----------------------------------------------------
			tokenizer = new StringTokenizer(lp.Sensors[3].getText(),"(), ", false);

			token = (tokenizer.hasMoreTokens()) ? tokenizer.nextToken() : "x position";

			success = false;
			while(!success)
			{
				try
				{
					xin=Double.parseDouble(token);
				}
				catch (NumberFormatException exception)
				{
					dialogInput = JOptionPane.showInputDialog(null,"Please reenter Sensor 4 x position.","Invalid x position",JOptionPane.ERROR_MESSAGE, null, null, token);
					try{token = dialogInput.toString();}
					catch(NullPointerException e){}
					continue;
				}
				success=true;
			}

			token = (tokenizer.hasMoreTokens()) ? tokenizer.nextToken() : "y position";

			success = false;
			while(!success)
			{
				try
				{
					yin=Double.parseDouble(token);
				}
				catch (NumberFormatException exception)
				{
					dialogInput = JOptionPane.showInputDialog(null,"Please reenter Sensor 4 y position.","Invalid y position",JOptionPane.ERROR_MESSAGE, null, null, token);
					try{token = dialogInput.toString();}
					catch(NullPointerException e){}
					continue;
				}
				success=true;
			}
			SLoc[0][3] = xin;
			SLoc[1][3] = yin;
			lp.Sensors[3].setText(Double.toString(xin)+", "+Double.toString(yin));

			//----------------------------------------------------
			//Get Sensor 5 position
			//----------------------------------------------------
			tokenizer = new StringTokenizer(lp.Sensors[4].getText(),"(), ", false);

			token = (tokenizer.hasMoreTokens()) ? tokenizer.nextToken() : "x position";

			success = false;
			while(!success)
			{
				try
				{
					xin=Double.parseDouble(token);
				}
				catch (NumberFormatException exception)
				{
					dialogInput = JOptionPane.showInputDialog(null,"Please reenter Sensor 5 x position.","Invalid x position",JOptionPane.ERROR_MESSAGE, null, null, token);
					try{token = dialogInput.toString();}
					catch(NullPointerException e){}
					continue;
				}
				success=true;
			}

			token = (tokenizer.hasMoreTokens()) ? tokenizer.nextToken() : "y position";

			success = false;
			while(!success)
			{
				try
				{
					yin=Double.parseDouble(token);
				}
				catch (NumberFormatException exception)
				{
					dialogInput = JOptionPane.showInputDialog(null,"Please reenter Sensor 5 y position.","Invalid y position",JOptionPane.ERROR_MESSAGE, null, null, token);
					try{token = dialogInput.toString();}
					catch(NullPointerException e){}
					continue;
				}
				success=true;
			}
			SLoc[0][4] = xin;
			SLoc[1][4] = yin;
			lp.Sensors[4].setText(Double.toString(xin)+", "+Double.toString(yin));


			//------------------------------------------
			//Get number of particles
			//------------------------------------------
			tryString = lp.Npart.getText();
			success = false;
			while(!success)
			{
				try
				{
					numParts = Integer.parseInt(tryString);
				}
				catch (NumberFormatException exception)
				{
					dialogInput = JOptionPane.showInputDialog(null,"Please reenter the number of particles.","Invalid number of particles",JOptionPane.ERROR_MESSAGE, null, null, tryString);
					try{tryString = dialogInput.toString();}
					catch(NullPointerException e){}
					continue;
				}
				success=true;
			}
			lp.Npart.setText(Integer.toString(numParts));

			//-------------------------------------------
			//Get number of time steps
			//-------------------------------------------
			tryString = lp.Timestep.getText();
			success = false;
			while(!success)
			{
				try
				{
					length = Integer.parseInt(tryString);
				}
				catch (NumberFormatException exception)
				{
					dialogInput = JOptionPane.showInputDialog(null,"Please reenter the number of time steps.","Invalid number of time steps",JOptionPane.ERROR_MESSAGE, null, null, tryString);
					try{tryString = dialogInput.toString();}
					catch(NullPointerException e){}
					continue;
				}
				success=true;
			}
			lp.Timestep.setText(Integer.toString(length));

			//-------------------------------------------
			//Get time increment
			//-------------------------------------------
			deltaT=lp.DeltaT.getSelectedIndex()+1;
			fps = 1.0/deltaT;
			pathErrorPanel.error=new double[length];
			measErrorPanel.error=new double[length];
			ploterror=false;
			pathErrorPanel.repaint();
			measErrorPanel.repaint();



			tokenizer = new StringTokenizer(lp.Xin.getText(),"(), ", false);

			token = (tokenizer.hasMoreTokens()) ? tokenizer.nextToken() : "x position";

			success = false;
			while(!success)
			{
				try
				{
					xin=Double.parseDouble(token);
				}
				catch (NumberFormatException exception)
				{
					dialogInput = JOptionPane.showInputDialog(null,"Please reenter x position.","Invalid x position",JOptionPane.ERROR_MESSAGE, null, null, token);
					try{token = dialogInput.toString();}
					catch(NullPointerException e){}
					continue;
				}
				success=true;
			}

			token = (tokenizer.hasMoreTokens()) ? tokenizer.nextToken() : "y position";

			success = false;
			while(!success)
			{
				try
				{
					yin=Double.parseDouble(token);
				}
				catch (NumberFormatException exception)
				{
					dialogInput = JOptionPane.showInputDialog(null,"Please reenter y position.","Invalid y position",JOptionPane.ERROR_MESSAGE, null, null, token);
					try{token = dialogInput.toString();}
					catch(NullPointerException e){}
					continue;
				}
				success=true;
			}

			tryString = lp.SensError.getText();
			success = false;
			while(!success)
			{
				try
				{
					r = Double.parseDouble(tryString);
				}
				catch (NumberFormatException exception)
				{
					dialogInput = JOptionPane.showInputDialog(null,"Please reenter sensor error variance.","Invalid sensor error",JOptionPane.ERROR_MESSAGE, null, null, tryString);
					try{tryString = dialogInput.toString();}
					catch(NullPointerException e){}
					continue;
				}
				success=true;
			}
			lp.SensError.setText(Double.toString(r));
			sqrtR = Math.sqrt(r);

			targetXmeter=xin;
			targetYmeter=yin;

			lp.Xin.setText(Double.toString(xin)+", "+Double.toString(yin));



			// Initialize arrays for the real and estimated paths and the sensor measurements.
			path=new double[4][length];
			xhat=new double[4][length];
			xmeas=new double[length];

			// Used by the thread.
			delay = (fps > 0) ? (int)(1000 / fps) : 100;
			path[0][0]=targetXmeter;
			path[1][0]=targetYmeter;
			path[2][0]=xvelocity;
			path[3][0]=yvelocity;
			xhat[0][0]=0;
			xhat[1][0]=0;
			xhat[2][0]=0;
			xhat[3][0]=0;

			xmax=targetXmeter;
			xmin=targetXmeter;
			ymax=targetYmeter;
			ymin=targetYmeter;
			parts = new particleList(numParts);
			resampled = new particleList(numParts);

			for (int i=0; i<numParts; i++)
			{
				parts.xPosition[i]=targetXmeter+1.73205*generator.nextGaussian();
				parts.yPosition[i]=targetYmeter+1.73205*generator.nextGaussian();
				parts.xVelocity[i]=xvelocity+1*generator.nextGaussian();
				parts.yVelocity[i]=yvelocity+1*generator.nextGaussian();
				xhat[0][0]=xhat[0][0]+parts.xPosition[i];
				xhat[1][0]=xhat[1][0]+parts.yPosition[i];
				xhat[2][0]=xhat[2][0]+parts.xVelocity[i];
				xhat[3][0]=xhat[3][0]+parts.yVelocity[i];
			}
			xhat[0][0]=xhat[0][0]/numParts;
			xhat[1][0]=xhat[1][0]/numParts;
			xhat[2][0]=xhat[2][0]/numParts;
			xhat[3][0]=xhat[3][0]/numParts;

			for (int i=1; i<length; i++)
			{
				double[] procnoise = new double[4];
				for (int randcount=0; randcount<4; randcount++)
				{
					procnoise[randcount] = Qval*generator.nextGaussian();
				}
				path[0][i] = path[0][i-1] + (1.0/fps)*path[2][i-1] + noise[deltaT-1][0]*procnoise[0] + noise[deltaT-1][1]*procnoise[1];
				path[2][i] = path[2][i-1] + noise[deltaT-1][1]*procnoise[0] + noise[deltaT-1][2]*procnoise[1];
				path[1][i] = path[1][i-1] + (1.0/fps)*path[3][i-1] + noise[deltaT-1][0]*procnoise[2] + noise[deltaT-1][1]*procnoise[3];
				path[3][i] = path[3][i-1] + noise[deltaT-1][1]*procnoise[2] + noise[deltaT-1][2]*procnoise[3];
				xmax = Math.max(xmax, path[0][i]);
				xmin = Math.min(xmin, path[0][i]);
				ymin = Math.min(ymin, path[1][i]);
				ymax = Math.max(ymax, path[1][i]);

				for(int scount=0; scount<NumSens; scount++)
				{
					xmax = Math.max(xmax, SLoc[0][scount]);
					xmin = Math.min(xmin, SLoc[0][scount]);
					ymax = Math.max(ymax, SLoc[1][scount]);
					ymin = Math.min(ymin, SLoc[1][scount]);
				}

 		 	}

			startThread();
		}


	}

	private class clearListener implements ActionListener
	{
		public void actionPerformed (ActionEvent event)
		{
			animator=null;
			// Sensor positions reset.
			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;


			// Jet (target) position is initially set at (500,500) meters.
			targetXmeter = 15;
			targetYmeter = 12;


			// The window is initially set to display from (0,0) to (1000,1000) meters.
			// These variables determine the range o