jtrackerguimote.java

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

					int y1 = d.height - (int)(((error[i-1])/(maxerror))*(d.height-10));
					int y2 = d.height - (int)(((error[i])/(maxerror))*(d.height-10));
					page.setColor(Color.red);
					page.drawLine(x1,y1,x2,y2);
					page.setColor(Color.black);
					page.drawLine(x1,d.height,x1,d.height-4);
					page.drawLine(x2,d.height,x2,d.height-4);
				}
				page.drawString("Max: " + dFormat.format(maxerror),2,16);
				page.drawString("Avg.: "+ dFormat.format(avgerror/length),120,16);
			}
		}
	}

	private class CheckPanel extends JPanel
	{
		public JCheckBox showPlane;
		public JCheckBox showOrg;
		public JCheckBox showRes;
		private JLabel checkLabel;
		private JLabel pauseLabel;
		public JCheckBox pauseBox;
		private JSeparator divLine, divLine2;
		public JButton resumeButton;
		public JCheckBox resBox;


		public CheckPanel()
		{
			setBackground(Color.lightGray);
			setBorder(BorderFactory.createEtchedBorder(EtchedBorder.LOWERED));
			checkLabel = new JLabel("Display:");
			pauseLabel = new JLabel("Pause between MC steps");
			showPlane = new JCheckBox("Target", true);
			showOrg = new JCheckBox("Particles", true);
			showRes = new JCheckBox("Resampled particles", true);
			pauseBox = new JCheckBox("Enable", false);
			resumeButton = new JButton("Resume");
			resBox = new JCheckBox("Use Resampling", true);
			showPlane.setBackground(Color.lightGray);
			showOrg.setBackground(Color.lightGray);
			showRes.setBackground(Color.lightGray);
			pauseBox.setBackground(Color.lightGray);
			resBox.setBackground(Color.lightGray);

			divLine = new JSeparator();
			divLine2 = new JSeparator();

/*			showPlane.setToolTipText("Check here to show the plane in the plot.");
			showOrg.setToolTipText("Check here to display the particles before resampling during animation.");
			showRes.setToolTipText("Check here to display the resampled particles during animation.");
			pauseBox.setToolTipText("Check here to pause animation between Monte Carlo Iterations.");
			resumeButton.setToolTipText("Click here to resume animation when paused.");
			resBox.setToolTipText("Check here to use resampling when tracking.");
*/
			showPlane.addMouseListener(new planemouse());
			showOrg.addMouseListener(new orgmouse());
			showRes.addMouseListener(new resmouse());
			pauseBox.addMouseListener(new boxmouse());
			resumeButton.addMouseListener(new resumemouse());
			resBox.addMouseListener(new resboxmouse());

			setLayout(new BoxLayout(this, BoxLayout.Y_AXIS));

			add(checkLabel);
			add(showPlane);
			add(showOrg);
			add(showRes);
			add(divLine);
			add(pauseLabel);
			add(pauseBox);
			add(resumeButton);
			add(divLine2);
			add(resBox);
			showPlane.addActionListener(new addListener());

			resumeButton.addActionListener(new resumeListener());

		}
	private class planemouse implements MouseListener
	{

			public void mousePressed(MouseEvent e) {}
			public void mouseReleased(MouseEvent e) {}
			public void mouseClicked(MouseEvent e) {}
			public void mouseEntered(MouseEvent e)
			{
				helpinfo.setText("Check here to show the target in the plot.");
			}
			public void mouseExited(MouseEvent e) {}

	}
		private class orgmouse implements MouseListener
		{

				public void mousePressed(MouseEvent e) {}
				public void mouseReleased(MouseEvent e) {}
				public void mouseClicked(MouseEvent e) {}
				public void mouseEntered(MouseEvent e)
				{
					helpinfo.setText("Check here to display the particles before resampling during animation.");
				}
				public void mouseExited(MouseEvent e) {}

	}
	private class resmouse implements MouseListener
	{

			public void mousePressed(MouseEvent e) {}
			public void mouseReleased(MouseEvent e) {}
			public void mouseClicked(MouseEvent e) {}
			public void mouseEntered(MouseEvent e)
			{
				helpinfo.setText("Check here to display the resampled particles during animation.");
			}
			public void mouseExited(MouseEvent e) {}

	}
	private class boxmouse implements MouseListener
	{

			public void mousePressed(MouseEvent e) {}
			public void mouseReleased(MouseEvent e) {}
			public void mouseClicked(MouseEvent e) {}
			public void mouseEntered(MouseEvent e)
			{
				helpinfo.setText("Check here to pause animation between Monte Carlo Iterations.");
			}
			public void mouseExited(MouseEvent e) {}

	}
	private class resumemouse implements MouseListener
	{

			public void mousePressed(MouseEvent e) {}
			public void mouseReleased(MouseEvent e) {}
			public void mouseClicked(MouseEvent e) {}
			public void mouseEntered(MouseEvent e)
			{
				helpinfo.setText("Click here to resume animation when paused.");
			}
			public void mouseExited(MouseEvent e) {}

	}
	private class resboxmouse implements MouseListener
	{

			public void mousePressed(MouseEvent e) {}
			public void mouseReleased(MouseEvent e) {}
			public void mouseClicked(MouseEvent e) {}
			public void mouseEntered(MouseEvent e)
			{
				helpinfo.setText("Check here to use resampling when tracking.");
			}
			public void mouseExited(MouseEvent e) {}

	}

		private class addListener implements ActionListener
		{
			public void actionPerformed(ActionEvent event)
			{
				if(animating)
					return;
				rp.repaint();
			}
		}

		private class resumeListener implements ActionListener
		{
			public void actionPerformed(ActionEvent eve)
			{
				if(paused)
				{
					xhat[0][0]=0;
					xhat[1][0]=0;
					xhat[2][0]=0;
					xhat[3][0]=0;
					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;

					//----------------------------------------------------
					//Get Sensor 1 position
					//----------------------------------------------------
					tokenizer = new StringTokenizer(lp.Sensors[0].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 1 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 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));