test_bootstrapfilter.cpp

一个很全的matlab工具包

#ifdef _BOOTSTRAPFILTER_
  filter_p = new BootstrapFilter<ColumnVector,ColumnVector> (Prior, 
							     arguments.resample_period,
							     0,
							     DEFAULT_RS);
#endif

  list<WeightedSample<ColumnVector> > samples(arguments.num_samples);
  list<WeightedSample<ColumnVector> >::iterator samples_it;
#ifdef __STREAM__
  ofstream aftersysupdate_f("data_visualisation/after_system_update.dat", ios::out);
  ofstream aftermeasupdate_f("data_visualisation/after_meas_update.dat", ios::out);

#ifdef __OCTAVE__
  if (aftersysupdate_f)
    {
      aftersysupdate_f << "# name: aftersysupdate\n# type: matrix\n"
		       << "# rows: " << (arguments.num_samples*(STATE_SIZE+1))*(arguments.num_time_steps-1) << "\n"
		       << "# columns: 1\n" << endl;
    }
  else { cerr << "MAIN: Error writing to file" << endl; }
  if (aftermeasupdate_f)
    {
      aftermeasupdate_f << "# name: aftermeasupdate\n# type: matrix\n"
			<< "# rows: " << (arguments.num_samples*(STATE_SIZE+1))*(arguments.num_time_steps-1) << "\n"
			<< "# columns: 1\n" << endl;
    }
  else { cerr << "MAIN: Error writing to file" << endl; }
#endif // __OCTAVE__
#endif // __STREAM__

  /*******************
   * ESTIMATION LOOP *
   *******************/
#ifdef __OPENDX__
  int object = 1;// keeps track of object that is written in the dxnative_samplefile
#endif
  for (current_time = 0; current_time < arguments.num_time_steps-1 ; current_time++)
    {
#ifdef __OPENDX__
      dxnative_samplefile << "\n##\n## Measurement " << current_time << "\n##\n";

#ifdef __SOCKET__
  int tempobject = 1; // keeps track of object written in tempdx
#endif // __SOCKET__

#endif // __OPENDX__
      
  if (arguments.verbose)
    cerr << "MAIN: Estimation loop " << current_time << "\n";
  
  // UPDATE FILTER
  update_matrix_B(current_time);
  filter_p->Update(sys_model,Inputs[current_time],meas_model,Measurements[current_time+1]);

#ifdef __DEBUG__
  post_p = filter_p->PostGet();
  samples = dynamic_cast<MCPdf<ColumnVector> *>(post_p)->ListOfSamplesGet();
  cerr << "MAIN: After Measurement Update\n";
  // KG debugging only
  cerr << "time : " << current_time << "\n";
  for (samples_it = samples.begin() ; samples_it != samples.end() ; samples_it++)
    {
      cerr << samples_it->ValueGet() << "Weight = "
	   << samples_it->WeightGet() << "\n";
    }
#endif //  __DEBUG__
#ifdef __STREAM__
  post_p = filter_p->PostGet();
  samples = dynamic_cast<MCPdf<ColumnVector> *>(post_p)->ListOfSamplesGet();
  for (samples_it = samples.begin() ; samples_it != samples.end() ; samples_it++)
    {
      aftermeasupdate_f << samples_it->ValueGet()
			<< samples_it->WeightGet() << " \n";
    }
#ifdef __OPENDX__

  // THIS ONE WRITES TO dxnative_samples
  // write the xy-positions of the samples
  dxnative_samplefile << "#\n# Object " << object
		      << ": xy-position of the samples\n#\n"
		      << "object " << object 
		      << " class array type float rank 1 shape 2 items " << samples.size()
		      << " data follows" << endl;
  for (samples_it = samples.begin() ; samples_it != samples.end() ; samples_it++)
    {
      dxnative_samplefile << samples_it->ValueGet()[X] << " "
			  << samples_it->ValueGet()[Y] << "\n";
    }
  object++;

  // write the orientation of the samples
  dxnative_samplefile << "#\n# Object " << object
		      << ": orientation of the samples\n#\n"
		      << "object " << object
		      << " class array type float rank 0 items " << samples.size()
		      << " data follows" << endl;
  for (samples_it = samples.begin() ; samples_it != samples.end() ; samples_it++)
    {
      dxnative_samplefile << samples_it->ValueGet()[THETA] << "\n";
    }
  dxnative_samplefile << "attribute \"dep\" string \"positions\"\n";
  object++;
      
  // write the weight of the samples
  dxnative_samplefile << "#\n# Object " << object
		      << ": weight of the samples\n#\n"
		      << "object " << object
		      << " class array type double rank 0 items " << samples.size()
		      << " data follows" << endl;
      
  for (samples_it = samples.begin() ; samples_it != samples.end() ; samples_it++)
    {
      dxnative_samplefile << samples_it->WeightGet() << "\n";
    }
      
  dxnative_samplefile << "attribute \"dep\" string \"positions\"" << endl;
  object++;

#ifdef __SOCKET__
  // THIS ONE CREATES A TEMP FILE, WITH NO SERIES, USED TO SEND TO SOCKET      

  ofstream tempdx(tempdx_location, ios::out);
  tempdx.precision(10);
  tempdx.width(10);
      
  if (tempdx)
    {
      tempdx << "# Data of the samples in DX native format, no series\n" << endl;
    }
  else 
    { 
      cerr << "MAIN: Error writing to DX files. Stopping program" << endl; 
      exit(-1); 
    }

  // write the xy-positions of the samples
  tempdx << "#\n# Object " << tempobject
	 << ": xy-position of the samples\n#\n"
	 << "object " << tempobject 
	 << " class array type float rank 1 shape 2 items " << samples.size()
	 << " data follows" << endl;
      
  for (samples_it = samples.begin() ; samples_it != samples.end() ; samples_it++)
    {
      tempdx << samples_it->ValueGet()[X] << " "
	     << samples_it->ValueGet()[Y] << "\n";
    }
  tempobject++;

  // write the orientation of the samples
  tempdx << "#\n# Object " << tempobject
	 << ": orientation of the samples\n#\n"
	 << "object " << tempobject
	 << " class array type float rank 0 items " << samples.size()
	 << " data follows" << endl;
  for (samples_it = samples.begin() ; samples_it != samples.end() ; samples_it++)
    {
      tempdx << samples_it->ValueGet()[THETA] << "\n";
    }
  tempdx << "attribute \"dep\" string \"positions\"\n";
  tempobject++;
      
  // write the weight of the samples
  tempdx << "#\n# Object " << tempobject
	 << ": weight of the samples\n#\n"
	 << "object " << tempobject
	 << " class array type double rank 0 items " << samples.size()
	 << " data follows" << endl;
      
  for (samples_it = samples.begin() ; samples_it != samples.end() ; samples_it++)
    {
      tempdx << samples_it->WeightGet() << "\n";
    }
      
  tempdx << "attribute \"dep\" string \"positions\"" << endl;
  tempobject++;

  tempdx << "\nobject " << tempobject << " class field\n"
	 << "component \"positions\" value " 
	 << 1
	 << "\ncomponent \"orientation\" value " 
	 << 2
	 << "\ncomponent \"weight\" value " 
	 << 3;

  tempdx << "\n##\n## The end \n##\nend" << endl;
  tempdx.flush();
  tempdx.close();

  // open ifstream to send to socket
  ifstream file(tempdx_location);
  if ( file )
    {
      // Let dxlinkclient know you are starting to send a serie
      if (send(new_fd, start_serie, strlen(start_serie)+1, 0) == -1)
	cerr << "send\n";
#ifdef __DEBUG__
      cerr << "START_SERIE\n";
#endif // __DEBUG

      while ( file.get(stream, MSGSIZE,'\0') )
	{
#ifdef __DEBUG__
	  cerr << stream << endl;
#endif // __DEBUG__
	  if (send(new_fd, stream, MSGSIZE-1, 0) == -1)
	    cerr << "send\n";
	}
    }
  if (send(new_fd, end_serie,strlen(end_serie)+1, 0) == -1)
    cerr << "send\n";
#ifdef __DEBUG__
  cerr << "END_SERIE\n";
#endif // __DEBUG__
  if (send(new_fd, start_update,strlen(start_update)+1, 0) == -1)
    cerr << "send\n";
  sleep (SLPTIME);
#endif // __SOCKET__
#endif // __OPENDX
#endif //  __STREAM__
    }

  // Show Final posterior
  cerr << "MAIN: Final Posterior\n";
  // KG debugging only
  post_p = filter_p->PostGet();
  cerr << "Expected Value = \n" << post_p->ExpectedValueGet() << "\n"
       << "Covariance = \n" << post_p->CovarianceGet() << "\n";

#ifdef __STREAM__
  aftersysupdate_f.close();
  aftermeasupdate_f.close();
#endif // __STREAM__

  /************************
   * CHECKING THE RESULTS *
   ************************/

#ifdef __DEBUG__
  cerr << "MAIN: Done\n" << endl;
#endif // __DEBUG__

  // VISUALISATION PART
#ifdef __STREAM__
#ifdef __OCTAVE__
  /* Octave native format to load variables is something of the form

  # name: varname
  # type: matrix (or scalar)
  # rows: NOR
  # columns: NOC
  DATA

  You can also choose the more general C-style IO functions, but this
  means more hassle to write the m-files for the reading of the data

  */
  ofstream constants_exp("data_visualisation/constants_oct.dat", ios::out);
  
  if (constants_exp)
    {
      constants_exp << "# name: DELTA_T\n# type: scalar\n" << DELTA_T << "\n\n" 
		    << "# name: NUM_TIME_STEPS\n# type: scalar\n" << arguments.num_time_steps << "\n\n" 
	// System Noise
		    << "# name: SIGMA_SYSTEM_NOISE\n# type: scalar\n" << SIGMA_SYSTEM_NOISE << "\n\n" 
		    << "# name: COR_SYSTEM_NOISE\n# type: scalar\n" << COR_SYSTEM_NOISE << "\n\n" 
	// Coordinates of wall
		    << "# name: RICO_WALL\n# type: scalar\n" << arguments.rico_wall << "\n\n" 
		    << "# name: OFFSET_WALL\n# type: scalar\n" << arguments.offset_wall << "\n\n" 
	// Measurement noise
		    << "# name: mu_meas_noise\n# type: scalar\n" << mu_meas_noise << "\n\n" 
		    << "# name: SIGMA_MEAS_NOISE\n# type: scalar\n" << SIGMA_MEAS_NOISE << "\n\n" 
		    << "# name: COR_MEAS_NOISE\n# type: scalar\n" << COR_MEAS_NOISE << "\n\n" 

	// Sizes
		    << "# name: STATE_SIZE\n# type: scalar\n" << STATE_SIZE << "\n\n" 
		    << "# name: INPUT_SIZE\n# type: scalar\n" << INPUT_SIZE << "\n\n" 
		    << "# name: MEAS_SIZE\n# type: scalar\n" << MEAS_SIZE << "\n\n" 

	// Inputs
		    << "# name: LINEAR_SPEED\n# type: scalar\n" << arguments.linear_speed << "\n\n" 
		    << "# name: ROT_SPEED\n# type: scalar\n" << arguments.rot_speed << "\n\n" 
	// Samples properties
		    << "# name: NUM_SAMPLES\n# type: scalar\n" << arguments.num_samples << "\n\n" 
		    << "# name: RESAMPLE_PERIOD\n# type: scalar\n" << arguments.resample_period << "\n\n" 
	// Prior
		    << "# name: PRIOR_MU_X\n# type: scalar\n" << PRIOR_MU_X << "\n\n" 
		    << "# name: PRIOR_MU_Y\n# type: scalar\n" << PRIOR_MU_Y << "\n\n" 
		    << "# name: PRIOR_MU_THETA\n# type: scalar\n" << PRIOR_MU_THETA << "\n\n" 
		    << "# name: PRIOR_COV\n# type: scalar\n" << PRIOR_COV << "\n\n" 
		    << "# name: PRIOR_COR\n# type: scalar\n" << PRIOR_COR << endl;

    }
#endif // __OCTAVE__

#ifdef __OPENDX__
#define NUM_COMPONENTS_PER_FIELD 3 // xy-position, orientation and weight

  unsigned int final_object = object;
  unsigned int numtimesteps = final_object/NUM_COMPONENTS_PER_FIELD;

  dxnative_samplefile << "\n\n##\n## Now Putting these objects together into fields\n##\n";
  for (current_time = 0; current_time < numtimesteps ; current_time++)
    {
      dxnative_samplefile << "\nobject " << object << " class field\n"
			  << "component \"positions\" value " 
			  << (current_time*NUM_COMPONENTS_PER_FIELD)+1
			  << "\ncomponent \"orientation\" value " 
			  << (current_time*NUM_COMPONENTS_PER_FIELD)+2
			  << "\ncomponent \"weight\" value " 
			  << (current_time*NUM_COMPONENTS_PER_FIELD)+3;
      object++;
    }

  dxnative_samplefile << "\n\n##\n## Now Putting these fields together into a series\n##\n";
  dxnative_samplefile << "\nobject \"series\" class series" << endl;

  for (current_time = 0; current_time < numtimesteps; current_time++)
    {
      dxnative_samplefile << "member " << current_time << " value "
			  << final_object+current_time
			  << " position " << current_time << endl;
    }
  
  dxnative_samplefile << "\n##\n## The end \n##\nend" << endl;

  dxnative_samplefile.close();
  cerr << dxfile_location << " has been written to disk.\n\n\n";


  /******************************************************
   *                                                    *
   * If you want to send the file with all the series,  *
   * uncomment the  code below and make sure you        *
   * specify the visualisation network  dx_vis.net in   *
   * dxlinkclient.cpp                                   *
   *                                                    *
   * You will have to comment out the code to send      *
   * series, or adapt the code in dxlinkclient.cpp      *
   *                                                    *
   ******************************************************/ 


// #ifdef __SOCKET__
//   file.open(dxfile_location);
//   if ( file )
//     {
//       if ( send(new_fd, start_file, strlen(start_file)+1, 0 ) == -1 ) 
// 	cerr << "send\n";
//       while ( file.get(stream, MSGSIZE,'\0') )
// 	{
// #ifdef __DEBUG__
// 	  cout << stream;
// #endif // __DEBUG__
// 	  if (send(new_fd, stream, MSGSIZE-1, 0) == -1)
// 	    cerr << "send\n";
// 	}
//       if (send(new_fd, end_file, strlen(end_file)+1, 0) == -1)
// 	cerr << "send\n";	
//       close(new_fd);
//       exit(0);
//     }
// #endif // __SOCKET__	  

#endif // __OPENDX__
#endif // __STREAM__
#ifdef __OPENDX__
#ifdef __SOCKET__
  if (send(new_fd, end, strlen(end)+1, 0) == -1)
    cerr << "send\n";
#endif // __SOCKET__
#endif // __OPENDX__
  cerr << "==================================================\n"
       << "End of the BootstrapFilter test \n"
       << "=================================================="
       << endl;

  ret = 0;
  return ret;
}