jtrackerguimote.java

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

					else
					{
						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;

						startThread();
						animating = true;
					}
				}

			}
		}
	}

// This method is called when the applet is no longer
	// visible. Set the animator variable to null so that the
	// thread will exit before displaying the next frame.
	public void stop()
	{
		animator = null;
		animating = false;
	}


	/*
	 * The RightPanel class contains the graphics component that plots the paths and particles.
	 * It also contains methods to allow dragging of the jet and radar images.
	 */
	private class RightPanel extends JPanel implements MouseListener, MouseMotionListener
	{
		private int x1, y1, x2, y2, x3, y3, xhat1, xhat2, yhat1, yhat2; // various pixel coordinates
		private double sumError, measureError;

     	private boolean dragging;		// This is true when dragging is in progress.
     	private int dragwhat;			// This int keeps track of what is being dragged.
 	    private int offsetx, offsety;	// The distance from the upper left corner of the
                                     	//   Sensor to the point where the user clicked
                                     	//   the rect.  This offset is maintained as the
                                     	//   rect is dragged.
        /* Constructor */
		public RightPanel()
		{
			//setBackground(Color.white);

			// Add a MouseListener to read mouse events (clicking and dragging) in the
			// graphics component.
			addMouseListener(this);
			addMouseMotionListener(this);

			// Sensor position is initially set at (0,0).


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

			// The window is initially set to display from (-15, -15) to (15, 15) meters.
			// These variables determine the range of the display, and are automatically
			// changed to keep the target path and the sensor on the display.
			xmin = -15;
			ymin = -15;
			xmax = 15;
			ymax = 15;

			sensorWidth=20;
			sensorHeight=27;


			SLocPixel = new int[2][5];
			distall = new double[5];



		}


		/* This method runs when the user clicks on the right panel. */
		public void mousePressed(MouseEvent evt)
		{
			// If dragging is already in progress, just return.
			if (dragging)
				return;

			// Start dragging the image that the user clicked (if any).
			if (!animating) //Don't allow dragging if it's animating.
			{
   				// Begin dragging sensor 1.
				if ( evt.getX() >= SLocPixel[0][0] && evt.getX() < SLocPixel[0][0] + sensorWidth && evt.getY() >= SLocPixel[1][0] && evt.getY() < SLocPixel[1][0] + sensorHeight )
				{
					dragging = true;
					dragwhat = 0;
					offsetx = evt.getX() - SLocPixel[0][0];
					offsety = evt.getY() - SLocPixel[1][0];
				}

   				// Begin dragging sensor 2.
				if ( evt.getX() >= SLocPixel[0][1] && evt.getX() < SLocPixel[0][1] + sensorWidth && evt.getY() >= SLocPixel[1][1] && evt.getY() < SLocPixel[1][1] + sensorHeight )
				{
					dragging = true;
					dragwhat = 1;
					offsetx = evt.getX() - SLocPixel[0][1];
					offsety = evt.getY() - SLocPixel[1][1];
				}

   				// Begin dragging sensor 3.
				if ( evt.getX() >= SLocPixel[0][2] && evt.getX() < SLocPixel[0][2] + sensorWidth && evt.getY() >= SLocPixel[1][2] && evt.getY() < SLocPixel[1][2] + sensorHeight )
				{
					dragging = true;
					dragwhat = 2;
					offsetx = evt.getX() - SLocPixel[0][2];
					offsety = evt.getY() - SLocPixel[1][2];
				}

   				// Begin dragging sensor 4.
				if ( evt.getX() >= SLocPixel[0][3] && evt.getX() < SLocPixel[0][3] + sensorWidth && evt.getY() >= SLocPixel[1][3] && evt.getY() < SLocPixel[1][3] + sensorHeight )
				{
					dragging = true;
					dragwhat = 3;
					offsetx = evt.getX() - SLocPixel[0][3];
					offsety = evt.getY() - SLocPixel[1][3];
				}

   				// Begin dragging sensor 5.
				if ( evt.getX() >= SLocPixel[0][4] && evt.getX() < SLocPixel[0][4] + sensorWidth && evt.getY() >= SLocPixel[1][4] && evt.getY() < SLocPixel[1][4] + sensorHeight )
				{
					dragging = true;
					dragwhat = 4;
					offsetx = evt.getX() - SLocPixel[0][4];
					offsety = evt.getY() - SLocPixel[1][4];
				}
				// Begin dragging the jet.  If the user clicks on a point occupied by both the jet and
				// the radar, the jet is selected.
				if ( evt.getX() >= targetX && evt.getX() < targetX + targetWidth && evt.getY() >= targetY && evt.getY() < targetY + targetHeight )
				{
					dragging = true;
					dragwhat = 5;
					offsetx = evt.getX() - targetX;
					offsety = evt.getY() - targetY;
				}
			}
			return;
		} // end mousePressed()


		/* End the drag operation, if one is in progress. */
		public void mouseReleased(MouseEvent evt)
 		{
 			if (dragging == false)
				return;
			dragging = false;
		} // end mouseReleased()


		/* Continue the drag operation if one is in progress. Move the image that is being dragged to the current
		 * mouse position.  But clamp it so that it can't be more than halfway off the screen. */
		public void mouseDragged(MouseEvent evt)
		{
			// If no dragging operation is in progess, just return.
			if (dragging == false)
            	return;

			// Dragging the sensor.
			if(dragwhat!=5)
			{
				// Get new postion of image.
				SLocPixel[0][dragwhat] = evt.getX() - offsetx;
				SLocPixel[1][dragwhat] = evt.getY() - offsety;

				// Clamp (x,y) to a permitted range, as described above.
				if (SLocPixel[0][dragwhat] < - sensorWidth / 2)
					SLocPixel[0][dragwhat] = - sensorWidth / 2;
				else if (SLocPixel[0][dragwhat] + sensorWidth/2 > getSize().width)
					SLocPixel[0][dragwhat] = getSize().width - sensorWidth / 2;
				if (SLocPixel[1][dragwhat] < - sensorHeight / 2)
					SLocPixel[1][dragwhat] = - sensorHeight / 2;
				else if (SLocPixel[1][dragwhat] + sensorHeight/2 > getSize().height)
					SLocPixel[1][dragwhat] = getSize().height - sensorHeight / 2;

				// Update the sensor position to match the position of the image.
				SLoc[0][dragwhat] = (SLocPixel[0][dragwhat]-50+sensorWidth/2)*(xmax-xmin)/(getWidth()-70)+xmin;
				SLoc[1][dragwhat] = (-SLocPixel[1][dragwhat]-20-sensorHeight/2+getHeight())*(ymax-ymin)/(getHeight()-40)+ymin;

				// Make the sensor position input fields reflect the new position.
				lp.Sensors[dragwhat].setText(dFormat.format(SLoc[0][dragwhat]) + ", " + dFormat.format(SLoc[1][dragwhat]));
			}

			// Dragging the jet.
			else
			{
				// Get new postion of image.
				targetX = evt.getX() - offsetx;
				targetY = evt.getY() - offsety;

				/* Clamp (x,y) to a permitted range, as described above. */
				if (targetX < - targetWidth / 2)
					targetX = - targetWidth / 2;
				else if (targetX + targetWidth/2 > getSize().width)
					targetX = getSize().width - targetWidth / 2;
				if (targetY < - targetHeight / 2)
					targetY = - targetHeight / 2;
				else if (targetY + targetHeight/2 > getSize().height)
					targetY = getSize().height - targetHeight / 2;

				// Update the target position to match the position of the image.
				targetXmeter = (targetX-50+targetWidth/2)*(xmax-xmin)/(getWidth()-70)+xmin;
				targetYmeter = (-targetY-20-targetHeight/2+getHeight())*(ymax-ymin)/(getHeight()-40)+ymin;

				// Make the initial position input fields reflect the new position.
				lp.Xin.setText(dFormat.format(targetXmeter) + ", " + dFormat.format(targetYmeter));
			}

			// Redraw the canvas, with the images in their new positions.
			repaint();
		}  // end mouseDragged()


		/* MouseEvent methods that aren't used are still defined. */
		public void mouseClicked(MouseEvent evt) { }
		public void mouseEntered(MouseEvent evt) { }
		public void mouseExited(MouseEvent evt) { }
		public void mouseMoved(MouseEvent evt) { }

		/* This is the method that does all the filtering calculations, and also displays the graphics.
		 * It is called using the repaint() method, or rp.repaint() if calling from outside the RightPanel
		 * class. */
		public void paintComponent(Graphics page)
		{
			super.paintComponent(page);
			setBackground(Color.white);
			Dimension d = new Dimension(getWidth(), getHeight());
			page.clearRect(0,0,d.width,d.height);
			wtotal=0;
			if (animating)
			{
				double totaldist = 0;
				for(int i=0; i<NumSens; i++)
				{
					distall[i] = Math.sqrt(Math.pow(path[0][frame] - SLoc[0][i],2) + Math.pow(path[1][frame] - SLoc[1][i],2));
					measureError = sqrtR*generator.nextGaussian();
					double dEp = distall[i] + epsilon;
					TrueMeas[i] = So/(dEp)+b+measureError;
					totaldist = totaldist + Math.abs(- dEp + dEp/(1+measureError*dEp/So));
				}

				measErrorPanel.error[frame] = totaldist/NumSens;

				for (int count=0; count<numParts; count++)
				{
					double[] procnoise = new double[4];
					for (int randcount=0; randcount<4; randcount++)
					{
						procnoise[randcount] = Qval*generator.nextGaussian();
					}
					parts.xPosition[count]=parts.xPosition[count] + parts.xVelocity[count]*1.0/fps + noise[deltaT-1][0]*procnoise[0] + noise[deltaT-1][1]*procnoise[1];
					parts.yPosition[count]=parts.yPosition[count] + parts.yVelocity[count]*1.0/fps + noise[deltaT-1][0]*procnoise[2] + noise[deltaT-1][1]*procnoise[3];
					parts.xVelocity[count]=parts.xVelocity[count] + noise[deltaT-1][1]*procnoise[0] + noise[deltaT-1][2]*procnoise[1];
					parts.yVelocity[count]=parts.yVelocity[count] + noise[deltaT-1][1]*procnoise[2] + noise[deltaT-1][2]*procnoise[3];
					sumError = 0;
					for(int i=0; i<NumSens; i++)
					{
						h_x[i] = b + So/(epsilon + Math.sqrt(Math.pow(parts.xPosition[count]-SLoc[0][i],2) + Math.pow(parts.yPosition[count] - SLoc[1][i],2)));
						e_x[i] = Math.pow(TrueMeas[i] - h_x[i],2);
						sumError = sumError + e_x[i];
					}
					parts.weight[count]=Math.exp(-0.5*(sumError/r));
					wtotal=wtotal+parts.weight[count];
				}

				xhat[0][frame]=0;
				xhat[1][frame]=0;
				xhat[2][frame]=0;
				xhat[3][frame]=0;

				for (int count=0; count<numParts; count++)
				{
					parts.weight[count] = (wtotal==0) ? 1.0/numParts : parts.weight[count]/wtotal;
					xhat[0][frame]=xhat[0][frame]+parts.xPosition[count]*parts.weight[count];
					xhat[1][frame]=xhat[1][frame]+parts.yPosition[count]*parts.weight[count];
					xhat[2][frame]=xhat[2][frame]+parts.xVelocity[count]*parts.weight[count];
					xhat[3][frame]=xhat[3][frame]+parts.yVelocity[count]*parts.weight[count];
				}

				resampled= (cp.resBox.isSelected()) ? parts.resample() : parts;
			}



			// Draw the sensors

			for(int j=0; j<NumSens; j++)
			{
				SLocPixel[0][j] = (int)((SLoc[0][j] - xmin)/(xmax-xmin)*(d.width-70)+50-sensorWidth/2);
				SLocPixel[1][j] =  d.height - (int)((SLoc[1][j]-ymin)/(ymax-ymin)*(d.height-40))-20-sensorHeight/2;
				page.drawImage(radar, SLocPixel[0][j], SLocPixel[1][j], sensorWidth, sensorHeight, null, null);
			}

			// Plot both sets of particles
			if(run)
			{
				resampled.pcolor=Color.yellow;
				parts.pcolor=Color.green;

				if(cp.showOrg.isSelected())
					parts.plotParticles(page,xmin,xmax,ymin,ymax,d);
				if(cp.showRes.isSelected())
					resampled.plotParticles(page,xmin,xmax,ymin,ymax,d);
				if(frame<length-1)
					parts=resampled;
			}



			if(run)
			{
				for (int count=1; count<=frame; count++)
				{
					x1 = (int)(((path[0][count-1]-xmin)/(xmax-xmin))*(d.width-70))+50;
					x2 = (int)(((path[0][count]-xmin)/(xmax-xmin))*(d.width-70))+50;
					y1 = d.height - (int)(((path[1][count-1]-ymin)/(ymax-ymin))*(d.height-40))-20;
					y2 = d.height - (int)(((path[1][count]-ymin)/(ymax-ymin))*(d.height-40))-20;
					page.setColor(Color.black);
					page.drawLine(x1,y1,x2,y2);

					xhat1 = (int)(((xhat[0][count-1]-xmin)/(xmax-xmin))*(d.width-70))+50;
					xhat2 = (int)(((xhat[0][count]-xmin)/(xmax-xmin))*(d.width-70))+50;
					yhat1 = d.height - (int)(((xhat[1][count-1]-ymin)/(ymax-ymin))*(d.height-40))-20;
					yhat2 = d.height - (int)(((xhat[1][count]-ymin)/(ymax-ymin))*(d.height-40))-20;
					page.setColor(Color.red);
					page.drawLine(xhat1,yhat1,xhat2,yhat2);
				}
				if (MC<niter)
				{
					for (int count = frame+1; count <= length-1; count++)
					{
						x1 = (int)(((path[0][count-1]-xmin)/(xmax-xmin))*(d.width-70))+50;
						x3 = (int)(((path[0][count]-xmin)/(xmax-xmin))*(d.width-70))+50;
						y1 = d.height - (int)(((path[1][count-1]-ymin)/(ymax-ymin))*(d.height-40))-20;
						y3 = d.height - (int)(((path[1][count]-ymin)/(ymax-ymin))*(d.height-40))-20;
						page.setColor(Color.black);
						page.drawLine(x1,y1,x3,y3);
					}
				}
			}
			if(animating)
			{
				targetX = x2-targetWidth/2;
				targetY = y2-targetHeight/2;
			}

			else
			{
				targetX = (int)((targetXmeter - xmin)/(xmax-xmin)*(d.width-70)+50-targetWidth/2);
				targetY = d.height - (int)((targetYmeter-ymin)/(ymax-ymin)*(d.height-40))-20-targetHeight/2;
			}
			if(cp.showPlane.isSelected())
			 	page.drawImage(jet,targetX, targetY,targetWidth,targetHeight,null,null);

			page.setColor(Color.black);
			page.drawLine(48,d.height-20,d.width-5,d.height-20);
			page.drawString(Integer.toString((int)ymin),2,d.height-17);
			page.drawLine(50,5,50,d.height-18);
			page.drawString(Integer.toString((int)xmin),35,d.height-5);
			page.drawLine(d.width-20,d.height-22,d.width-20,d.height-18);
			page.drawString(Integer.toString((int)xmax),d.width-35,d.height-5);
			page.drawLine(48,20,52,20);
			page.drawString(Integer.toString((int)ymax),2,25);


		}
	}


	private class EPanel extends JPanel
	{
		public double error[];
		public double maxerror;
		public double avgerror;

		public EPanel()
		{
			error=new double[length];
		}

		public void paintComponent(Graphics page)
		{
			super.paintComponent(page);
			setBackground(Color.white);
			Dimension d = new Dimension(getWidth(), getHeight());
			page.clearRect(0,0,d.width,d.height);
			maxerror=0;
			avgerror=0;

			if(ploterror)
			{
				for(int i=0; i<length; i++)
				{
					maxerror=Math.max(maxerror,error[i]);
					avgerror = avgerror + error[i];
				}
				for(int i=1; i<length; i++)
				{
					int x1 = (int)(((float)(i-1.0)/(length-1.0))*(d.width-20))+10;
					int x2 = (int)(((float)(i*1.0)/(length-1.0))*(d.width-20))+10;