test_bootstrapfilter.cpp

一个很全的matlab工具包

// $Id: test_bootstrapfilter.cpp,v 2.43 2005/01/06 14:30:53 kgadeyne Exp $
// Copyright (C) 2003 Klaas Gadeyne <klaas dot gadeyne at mech dot kuleuven dot ac dot be>
//  
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//  
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//  
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
//  
// Socket code added by Jan Callewaert

/* Primary demonstration program for the Bayesian Filtering Library.
   Mobile robot localization with a particle filter.  Use
   ./testbootstrapfilter.out --help 
   to see the possible command line arguments...
*/

/* IMPORTANT NOTE FROM THE AUTHOR */
/* If you look at this "tutorial/demo" program for the first time, you
   might get overwhelmed by the amount of code.  However, there is a
   lot of code contained in this example which has nothing to do with
   the estimation process in se and only with visualisation,
   debugging, ...
   All that code is put between #ifdef statements.  For first time
   study/understanding of this application, you can better eliminate
   this code.
   Emacs users:
   - M-x hide-ifdef-mode to enter ifdef minor mode
   - C-c @ C-d will hide code between ifdef statements (notice the
   ellipsis if it worked)
   - C-c @ C-s will show hidden code between ifdef statements
*/

#include <filter/bootstrapfilter.h>
#include <filter/asirfilter.h>
#include <filter/EKparticlefilter.h>
#include <model/linearanalyticsystemmodel_gaussianuncertainty.h>
#include <model/linearanalyticmeasurementmodel_gaussianuncertainty.h>
#include <pdf/analyticconditionalgaussian.h>
#include <pdf/gaussian.h>
#include <matrix_wrapper.h>
#include <vector_wrapper.h>
#include <iostream>
#include <fstream>

// The following headers are only neccessary when using opendx and
// sending the data over a socket

#define _BOOTSTRAPFILTER_
// #define _ASIRFILTER_
// #define _EK_PARTICLEFILTLER_

#ifdef __STREAM__
#ifdef __OPENDX__
#include <cstdlib>

#ifdef __SOCKET__
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/wait.h>

#include <netinet/in.h>
#include <arpa/inet.h>

#include <unistd.h>
#include <errno.h>
#endif // __SOCKET__

#include <string>
#endif // __OPENDX__
#endif // __STREAM__

#include "parser.h"
#include "mobile_robot_particlefilter.h"

using namespace std;
using namespace MatrixWrapper;

#ifdef __SOCKET__

// Socket descriptions
#define DEFAULT_PORT 3490  // Default port
#define BACKLOG 10         // how many pending connections queue will hold
#define MSGSIZE 100        // Size of a buffer when sending a file
#define SLPTIME 2

#ifdef __OPENDX__

// Set socket connection
void init_connection( int * sin_size,int * sockfd, int port )
{
  int yes=1;
  struct sockaddr_in my_addr;    // my address information

  if ((*sockfd = socket(AF_INET, SOCK_STREAM, 0)) == -1) {
    cerr << "socket error\n";
    exit(1);
  }

  if (setsockopt(*sockfd,SOL_SOCKET,SO_REUSEADDR,&yes,sizeof(int)) == -1) {
    cerr << "setsockopt error\n";
    exit(1);
  }
        
  my_addr.sin_family = AF_INET;         // host byte order
  my_addr.sin_port = htons(port);       // short, network byte order
  my_addr.sin_addr.s_addr = INADDR_ANY; // automatically fill with my IP
  memset(&(my_addr.sin_zero), '\0', 8); // zero the rest of the struct


  // Bind socket descriptor
  if (bind(*sockfd, (struct sockaddr *)&my_addr, sizeof(struct sockaddr))
      == -1) {
    cerr << "bind error\n";
    exit(1);
  }

  // Listen to socket
  if (listen(*sockfd, BACKLOG) == -1) {
    cerr << "listen error\n";
    exit(1);
  }
  cout << "connection made to port " << port << endl;
}
#endif // __SOCKET__
#endif // __OPENDX__

int main(int argc, char **argv)
{
  cerr << "==================================================\n"
       << "Test of the BootstrapFilter"
       << "Mobile robot localisation example\n"
       << "=================================================="
       << endl;

  // For the parsing of command line variables
  /* Set argument defaults */
  struct arguments arguments;
  arguments.verbose = 0;
  arguments.num_samples = DEFAULT_NUM_SAMPLES;
  arguments.num_time_steps = DEFAULT_NUM_TIME_STEPS; 
  arguments.resample_period = DEFAULT_RESAMPLE_PERIOD;
  arguments.linear_speed = DEFAULT_LINEAR_SPEED;
  arguments.rot_speed = DEFAULT_ROT_SPEED;
  arguments.rico_wall = DEFAULT_RICO_WALL;
  arguments.offset_wall = DEFAULT_OFFSET_WALL;
  arguments.name = "new_experiment";
#ifdef __SOCKET__
  arguments.port = DEFAULT_PORT;
#endif // __SOCKET__

  /* Where the magic happens */
  int ret = argp_parse (&argp, argc, argv, 0, 0, &arguments);
  if (ret != 0) 
    {
      cerr << "Parsing error" << endl;
      return ret;
    }

  cout << "****************************\n"
       << "*  name: " << arguments.name << endl
       << "*  num_samples: " << arguments.num_samples << endl
       << "*  num_time_steps: " << arguments.num_time_steps << endl
       << "*  resample_period: " << arguments.resample_period << endl
       << "*  linear_speed: " << arguments.linear_speed << endl
       << "*  rot_speed: " << arguments.rot_speed << endl
#ifdef __SOCKET__
       << "*  port: " << arguments.port << endl
#endif // __SOCKET__
       << "*  rico_wall: " << arguments.rico_wall << endl
       << "*  offset_wall: " << arguments.offset_wall << endl
       << "****************************\n\n";

  // Measurement Noise
  wall_ct = 1/(sqrt(pow(arguments.rico_wall,2.0) + 1));
  mu_meas_noise = arguments.offset_wall * wall_ct;

#ifdef __STREAM__
#ifdef __OPENDX__

  char dxfile_location[128] = "data_visualisation/filter_samples.dx";

#ifdef __SOCKET__
  char tempdx_location[128] = "data_visualisation/temp.dx";

  // Socket parameters
  int sockfd, new_fd;            // listen on sock_fd, new connection on new_fd
  struct sockaddr_in their_addr; // connector's address information
  int sin_size;

  // Setup the socket
  init_connection( &sin_size, &sockfd, arguments.port );
  // Accept the connection
  sin_size = sizeof(struct sockaddr_in);
  if ( (new_fd = accept(sockfd, (struct sockaddr *)&their_addr,
			(socklen_t *)&sin_size)) == -1) 
    {
      cerr << "accept";
    }
  if (arguments.verbose == 1)
    {
      cout << "server: got connection from " 
	   << inet_ntoa(their_addr.sin_addr) << endl;
    }

  close(sockfd); // child doesn't need the listener
  char buf[20];
  char *start_name = "STR_NAM";
  char *start_rico_wall = "STR_RIC";
  char *start_offset_wall = "STR_OFF";
  char *start_msgsize = "STR_MSG";
  char *start_serie = "STR_SER";
  char *end_serie = "END_SER";
  char *start_update = "STR_UPD";
//   char *start_file = "STR_FIL";
//   char *end_file = "END_FIL";
  char *end = "END";
  
  // Send the name of the experiment
  if (send(new_fd,start_name, strlen(start_name)+1, 0) == -1)
    cerr << "send\n";  
  sleep(SLPTIME);
  if (send(new_fd, arguments.name, strlen(arguments.name)+1, 0) == -1)
    cerr << "send\n";
  sleep(SLPTIME);


  // First send the rico_wall
  if (send(new_fd,start_rico_wall, strlen(start_rico_wall)+1, 0) == -1)
    cerr << "send\n";
  sleep(SLPTIME);
  gcvt(arguments.rico_wall,10,buf); // you have to convert double to string
  cerr << "strlen(buf): " << strlen(buf) << endl;
  if (send(new_fd, buf, strlen(buf)+1, 0) == -1)
    cerr << "send\n";
  sleep(SLPTIME);

  // Then send the offset_wall
  if (send(new_fd,start_offset_wall, strlen(start_offset_wall)+1, 0) == -1)
    cerr << "send\n";
  sleep(SLPTIME);
  gcvt(arguments.offset_wall,10,buf);
  if (send(new_fd, buf, strlen(buf)+1, 0) == -1)
    cerr << "send\n";
  sleep(SLPTIME);

  // We send the file in buffers of size MSGSIZE
  if (send(new_fd,start_msgsize, strlen(start_msgsize)+1, 0) == -1)
    cerr << "send\n";
  sleep(SLPTIME);
  sprintf(buf,"%i",MSGSIZE);
  cerr << "strlen(msgsize): " << strlen(buf) << endl;
  if (send(new_fd, buf, strlen(buf)+1,0) == -1 )
    cerr << "send\n";
  sleep(SLPTIME);


  char stream[MSGSIZE]; // contains stream read from file	  
  ifstream file;        // ifstream that contains file to open for writing

#endif // __SOCKET__
#endif // __OPENDX__
#endif // __STREAM__

  init_system_model();
  init_measurement_model();
  init_prior();

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

#endif // __OPENDX__
#endif // __STREAM__


  // Creating a MONTE CARLO density with these samples
#ifdef __STREAM__
  cerr << "MAIN: Creating the Prior density" << endl;
#endif // __STREAM__

  init_mc_prior(arguments.num_samples);

#ifdef __STREAM__
  ofstream priorsamples_f("data_visualisation/prior_samples.dat", ios::out);
  list<Sample<ColumnVector> >::iterator prior_it;
  if (priorsamples_f)
    {
#ifdef __OCTAVE__
      priorsamples_f << "# name: priorsamplesvector\n# type: matrix\n"
		     << "# rows: " << arguments.num_samples*(STATE_SIZE) << "\n"
		     << "# columns: 1\n" << endl;
#endif // __OCTAVE__
      for (prior_it = prior_samples.begin() ; prior_it != prior_samples.end() ; prior_it++)
	{
	  priorsamples_f << prior_it->ValueGet();
	}
    }
  else 
    { 
      cerr << "MAIN: Error writing to file" << endl;
    }
  priorsamples_f.close();
#endif // __STREAM__

  /*****************************************
   * SIMULATION OF INPUTS AND MEASUREMENTS *
   *****************************************/
  simulate_measurements(arguments.num_time_steps,
			arguments.linear_speed,
			arguments.rot_speed);


#ifdef __STREAM__
  cerr << "MAIN: Starting the simulation of the measurements" << endl;
  ofstream simulation_f("data_visualisation/simulation.dat", ios::out);
  if (simulation_f)
    {
#ifdef __OCTAVE__
      simulation_f << "# name: simulation_states\n# type: matrix\n"
		   << "# rows: " << (STATE_SIZE *(arguments.num_time_steps)) << "\n"
		   << "# columns: 1\n" << endl;

#endif // __OCTAVE_
      for (current_time=0; 
	   current_time < arguments.num_time_steps;
	   current_time++)
	{
  // #define WEIGHT 1
	  simulation_f << States[current_time]; // << WEIGHT << " \n";
	}
    }
  else { cerr << "MAIN: Error writing to file" << endl; }
#endif // __STREAM

// #ifdef __DEBUG__  
//  cerr << "\nMAIN: states 0\n" << States[0] << endl;
//  double probability = meas_model.ProbabilityGet(Measurements[0],
// 						States[0]);
//  cerr << "\nMAIN: Meas 0 : "<< Measurements[0]
//       << "Prob : " << probability << "\n";
// #endif //  __DEBUG__

// #ifdef __STREAM__
//   // #define WEIGHT 1
//        simulation_f << States[current_time+1]; // << WEIGHT << " \n";
// #endif //  __STREAM__


  /******************************
   * Construction of the Filter *
   ******************************/
#ifdef __STREAM__
  cerr << "\nMAIN: Construction of the Bootstrapfilter: Post MCPdf will be created" << endl;
#endif // __STREAM__
#ifdef _ASIRFILTER_
  cerr << "MAIN: Using the Auxiliary Particle Filter" << endl;
  filter_p = new ASIRFilter<ColumnVector,ColumnVector> (Prior, 
							0,
							DEFAULT_RESAMPLE_THRESHOLD,
							DEFAULT_RS);
#endif
#ifdef _EK_PARTICLEFILTLER_
  cerr << "MAIN: Using the Extended Kalman Particle Filter" << endl;
  filter_p = new EKParticleFilter(Prior, 0, DEFAULT_RESAMPLE_THRESHOLD,
				  DEFAULT_RS);
#endif